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TION  AND  INTERPRETATION^  AND  d)  REPRESENTATION  OF  SCENE  DATA. 
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SUMMARY  PERSPECTIVES  OF  CONTROL  ISSUES 


BASIC  DEFINITIONS 
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SOME  DATA  ABOUT  INDUSTRIAL  ROBOTS  (December.  1980) 
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INDUSTRIAL  ROBOT  AND  OTHER  MANIPULATOR  EXAMPLES 
INCLUDING  WORKSPACE  AND  APPLICATION  CONCEPT  DESCRIPTIONS 
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COORDINATE  FRAMES  AND  PARAMETERS 


relative  to  frame  i-l.  The  upper  right  3X1  partition  expresses  the 
position  of  the  origin  of  frame  i  relative  to  i-l. 


USING  THE  HARTENBERG  -  DENAVIT  REPRESENTATION 


Note:  The  ranges  of  the  six  joint  variables  are  indicated  in  brackets  above. 


REFERENCE  FRAMES  FOR  LINK- JOINT  PAIRS  OF 


STANFORD  ARM 


JPL  -  STANFORD  ARM 


The  arm  transformation  T  is  given  by 


Arm  Transformation 


COS0,  cosd,  CO80.  ,  etc. 


OTHER  EXAMPLE  FOR  H-D  MATRIX  REPRESENTATION 
(UN I MATE  ROBOT  ARM) 
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ROBOT  MANIPULATOR  DYNAMIC  EQUATIONS 
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THE  4  X  4  PSEUDO  INERTIA  MATRIX 


"j"  about  the  origin  of  the  coordinate  frame  fixed  in 
the  same  body  (link). 


EXAMPLE;  DYNAMIC  EQUATIONS  FOR  THE  JPL  -  STANFORD  ARM 
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THE  DYNAMIC  PROFILE  OF  A  PATH-CONTROLLED  MOTION 
OF  THE  JPL-STANFORD  ARM.  (THE  CORRESPONDING  JOINT 


Torque  and  Force  Variations 


FURTHER  "NATURAL"  SIMPLIFICATIONS  OF  THE  DYNAMIC 
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configuration  of  proximity  sensors  on  four-claw  end 
effector.  Right:  four-sensor  operation  concept  for 
simultaneous  measurement  of  depth,  pitch  and  yaw  errors.) 
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end  sets  up  standing  waves  within  the  cavity.  As  the  standing  wave  pattern 
moves,  it  changes  the  sound  pressure  which  is  detected  by  a  microphone. 

Another  type  of  acoustic  proximity  sensor,  based  on  the  sonar  principle,  is 
used  in  Polaroid  cameras.  The  sensor  and  associated  electronics  are  available 
for  under  $200. 
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bonded  bar  semiconductor  strain  qage  is  made  of  silicon  elements  bonded  to 
a  cantilever  beam  or  diaphragm.  The  diffused-type  semiconductor  strain 
gages  are  made  using  photolithographic  and  diffusion  techniques  similar  to 
those  applied  in  manufacturing  integrated  circuits. 


The  capacitance  pressure  transducers  use  a  moving  diaphragm  positioned 
between  two  fixed  plates.  Motion  of  the  diaphragm  as  a  result  of  applied 
pressure  or  force  will  cause  a  change  of  capacitance  in  two  circuits  which 
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The  characteristics  and  performance  of  various  pressure  transducers 
are  sutiriarlzed  in  two  tables  (Tables  5-2  and  5-3). 


Table  5-2.  Summary  of  Pressure/Force  Transducer  Characteristics  [5.2-1] 
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Table  5-3.  Comparative  Performance  Ranking  of  Pressure/Force 
Transducers 


Small  Size 

1.  Piezoelectric 

2.  Diffused  Semiconducfor 
Strain  Gage 

3.  Unbonded  Strain  Cage 

4.  Oonded  Ba'  SeiTiCOnductor 
Strain  Gage  - 

5.  Thin  Film  Strain  Gage 

6.  Variable  Reluctance 

7.  Bonded  Foil  Strain  Gage 

8.  Potentiometer 

9.  Capacitance 

10.  Oitlerential  Transformer 
U.  Vibrating  Wire  or  Tube 
12.  Force  Balance 


Fregitancy  ReapenM 

1.  Piezoelectric 

2.  Unbortded  Strain  Gage 

3.  Oiffused  Semiconductor 
Strain  Gage 

4.  Thin  Film  Strain  Gage 

5.  Bonded  Bar  Semiconductor 
Strain  Gage 

6.  Bonded  Foil  Strain  Gage 


Long-Term 

Calibration  Stability 

1.  Vibrating  Wire  oi  Tube 
2  Capacitance 
3.  Trim  Film  Strain  Gage 

4  Oilfusnd  Scmi£oiirti)<.lor 
Strain  Gage 

5  BorKled  fJ.li  SemiLonouctor 
Strain  Gage 

6  Bunded  Foil  Strain  Gage 

7  Unbonded  Strain  Gage 

8.  Variable  Reluctance 

9.  OiMereniial  Transformer 
10.  Piezoelectric 

It.  Poiciilionietpf 
12  Force  Balance 


Small  Static  Error 

1.  Vibrating  Wire  or  Tube 

2.  Force  Balance 

3.  Capacitance 

4.  Thin  Film  Strain  Gage 

5.  Oiffused  Semiconductor 
Strain  Gage 

6.  Unbonded  Strain  Gage 

7.  Bonded  Bar  Semiconductor 


o 


Output  Level 

1.  Force  Balance 

2.  Potentiometer 

3.  Capacitance 

4.  Vibrating  Wire  or  Tube 

5.  Differential  Translormer 

6.  Variable  Reluctance 

7.  Piezoelectric 

8.  Diffused  Sem.conduclor 
Strain  Gage 

9.  Bonded  Bar  Semiconductor 
Strain  Gage 

10.  Unbonded  Strain  Gage 
1 1  Thin  Film  Strain  Gage 
12.  Bunded  Foil  Strain  Gage 


Low  Cost 
in  Small  Quantity 

1.  Bonded  Foil  Strain  Gage 

2.  Unbonded  Strain  Gage 

3.  Potentiometer 

4.  Differential  Transformer 

5.  Variable  Reluctance 

6.  Thin  Film  Strain  Gage 

7.  Bonded  Bar  Semiconductor 
Strain  Gage 


7.  Variable  Reluctance 

Strain  Gage 

8. 

Piezoelectric 

8.  Capacitance 

8.  Bonded  Foil  Strain  Gage 

9. 

Oiffused  Semiconductor 

9.  Vibrating  Wire  Of  Tube 

10.  Differential  Transformer 

9.  Variable  Reluctance 
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Strain  Gage 
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11.  Piezoelectric 

11. 
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Figure  5-3.  Use  of  Strain  Gages  on  Cross-Beam  Force  Sutnrrim 
Elements  for  Six-Dimentional  Force-Torque  Sensing 
[5.1-1]. 
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Figure  5-4.  Use  of  Strain  Gages  on  Three  Equally  Soaced  Lens  Sup¬ 
ported  by  Cylindrical  Disks  or  Rings  fc'  Six-Dimensional 
Force-Torque  Sensing.  (From  [5.2-2]  anc  U.S.  Patent  No. 
4.094,192.) 
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New  entries  into  the  field  of  tactile  sensing  include  piezo-resistive 
transducers  using  bonded  doped-silicon  bars  and  silicon-doped  resistive 
elements,  formed  by  IC  techniques.  Another  new  sensing  concept  at  JPL  uses 
infrared  light  and  fiber  optics  [5.3-2]  as  illustrated  in  Fig.  5-7. 


Figure  5-6.  Use  of  pressure- conductive  Elastomer  for  Touch 
sensing  at  OPL  (From.  Ref.  5.1-1). 


References  [5.3-3  through  5.3-46]  list  useful  literature  in  the  field  of 
touch  sensing.  Touch  sensing  technology  -  in  a  broad  sense  -  has  been 
reviewed  recently  in  [5.3-47] 
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Figure  5-8  shows  the  concept  of  an  omnidirectional  slip  sensor  developed 
at  JPL.  This  bench-model  sensor  detects  slip  from  any  direction,  identifies 
the  slip  direction  within  20  deg  annular  segment,  and  measures  the  rate  of 
slip.  Further  information  on  this  sensor  can  be  found  in  [5.1-1]. 


results  when  a  force  is  applied.  Projectinq  this  point  to  the  leading  edge 
of  a  shaft  to  be  inserted  into  a  hole  is  the  basis  for  the  RCC  performing 
its  function  [5.4-62].  Figures  5-9  and  5-10  illustrate  two  somewhat  differ¬ 
ent  compliance  devices  available  commercially. 
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5.2-4.  Astek  Sales  Brochure,  Astek  Engineering,  Inc.,  Watertown, 
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GENERAL  ELEMENTS  OF  A  COMPUTER  CONTROL  SYSTEM 


COORDIMATE  TRANSFORMATIONS  IN  AN  INTERACTIVE  SENSOR-REFERENCED  ROBOT 


(IrU) 


CONTROL  INFLUENCE  ON  RESOLUTION  ACCURACY/SPATIAL  RESOLUTION  RELATIONSHIP 


CONTROL  -  ACCURACY  -  REPEATABILITY 
(USEFUL  REMARKS) 
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ABSTRACT 


A  structurad  approach  to  the  design  and  implementation 
of  interactive  sensor  refarencad  manipulator  control  sys~ 
terns  is  proposed.  Tha  approach  saparatas  the  algorithna 
that  perform  the  manipulstor  cask  controls  from  the  local 
manipulator  servo  control.  According  to  this,  manipulator 
control  is  being  considarad  as  the  problem  of  axacution  of 
an  action  nacwork.  Any  manipulstor  cask  is  traacad  as  an 
arrangement  of  incarconnscced  acciona  grouped  into  three 
categories:  primitive,  composite  and  complex  actions.  Tha 
sofeuare  implamencacion  of  all  three  action  cacagorias  la 
discussed.  The  concept  of  cooperative  processes  and  moni¬ 
tors  has  baen  adopted  as  a  powerful  method  of  software 
organization.  The  control  system  is  designed  and  demon- 
scracad  for  a  class  of  casks  chat  Include  aearching,  fol¬ 
lowing,  grasping,  and  stopping  of  slowly  moving  heavy 
objaecs  so  chat  operator  and  compucar  share  the  control. 

Tha  damonstration  hardwars  is  the  six-dagrac-of-freedom 
CURV  sianlpulacor  Incegracad  with  proximity  and  force- 
torque  stnsors  at  the  JPL  teleoparacor  laboratory. 


Frequent  Abbreviations 


MCS:  Manipulator  Control  System 


CACSs  CURV  Arm  Control  System 


EE:  End  Effector 


JCSi  Joint  Coordinacs  System 

weS:  World  Coordinate  System 

TCS:  Tracking  (Plana)  Coordinate  System 

HCS:  Hand  Coordinate  System 

TP:  Target  Plana 

APC:  Action  Fracedanca  Graph 

1 .  INTRODUCTION 


A  pilot  control  system  has  bean  davalopad  for  a  slx- 
degrea-of-frsadom  manipulator  equipped  with  proximity  and 
force-corqua  sanaors  in  the  JPL  talaoparscor  laboratory. 
The  control  systam,  implamanced  in  a  dadicacad  minicom- 
pucar,  allows  an  interactive  manual  and  automatic  control 
of  tha  manipulator.  In  this  papar,  "intarsetiva  control" 
signifias  a  hybrid  control  capability  wharaby  soma  motions 
of  tha  manipulator  in  tha  work  specs  ars  undsr  manual 
control  whils  the  remaining  motions  era  under  automatic 
computer  control  rafarcoead  to  proximity  and  forca-torqua 
sensor  data.  "Motion  in  work  space"  signifias  the  thraa 
translational  and  thrsa  rotational  motions  of  tha  end 
af factor.  Ths  operator  dscidas  which  work  space  motion  is 
in  manual  or  automatic  control.  In  extrema  csssa  all 
control  can  ba  fully  manual  or  fully  automatic.  Ths 
opsrator's  decisions  sra  communicated  to  ths  control  com- 
putsr  through  spproprlats  function  (logic)  switchss.  As  a 
consaquanca,  tha  oparstor  has  a  dual  (analog  and  logic) 
comnlcation  with  the  intarsetiva  control  systam. 

Tha  conceptual  and  Implemantatlonal  intricsclas  of 
such  an  intaractiva  Manipulstor  Control  System  (MCS)  can 


best  be  appraciatsd  by  noting  chat  each  motion  of  the  end 
effector  In  the  work  space  usually  requires  the  coordi¬ 
nated  control  of  several  manipulator  Joints.  In  general, 
therefore,  control  refaranca  Inputs  to  a  manipulator  joint 
will  be  computed  from  commands  originating  simultaneouely 
from  manual  (analog)  sources  and  from  compucar  algorithms 
refarencad  to  proximity  or  force-torque  aanaor  data.  The 
computation  of  control  raferanca  Inputs  addressed  to  Che 
individual  manipulator  jolnca  will  depend  on  the  setting 
of  the  function  (logic)  awlcches. 

A  conventional  system  control  concept  would  consider 
Che  inceraccivs  manipulator  control  as  a  taak  planning  and 
decompoaiclon  problem.  (See,  e.g.,  Rafs.  1-6.)  That 
concept  usually  culminates  in  the  formulation  of  special 
compucar  "languages"  and  procedural  or  transformation  nets 
for  manipulator  control,  and  defines  operator  Interaction 
with  the  control  system  as  an  essentially  off-llna  and 
high  Icval  programming  action.  The  taak  planning/ 
decomposition  concape  is  mostly  suited  to  cases  when  tha 
operator  wants  to  trade  control  (Raf.  7)  with  tha  computer. 
In  contrary  to  chat,  tha  cases  considered  in  this  papar 
era  those  whan  the  oparator  wants  to  share  control  (Ref.  7) 
with  tha  computer.  In  shared  control  the  oparator  has  an 
essentially  on-lina  Incaractlon  with  tha  control  system, 
and  tha  symbolic  elements  of  tha  on-line  intaraccion  should 
prafarably  ba  reduced  to  simple  and  discrata  function  or 
logic  commands. 

A  usual  approach  to  Che  design  and  implamancation  of 
sensor-refarancad  Incaraccivc  manipulator  control  would 
erase  Che  problem  as  a  monolithic  multlvarlabla  feedback 
control  systam  whare  the  dynemic  and  servo  characteristics 
of  Che  manipulator  play  an  eseencial  role  in  synthesizing 
Che  control  algorithms.  (See,  e.g..  Refs,  8  -  12.)  Such 
an  approach,  however,  would  ovarehadow  tha  problem  of 
synthesizing  tha  control  for  a  mulclcuda  of  manipulator 
casks.  It  is  noted  chat  Che  implementation  of  manipulator 
task  controls  are  mostly  based  on  discrete  evancs  and 
logic  decisions  which  esnnot  easily  be  considered  within 
Che  frame  of  Che  claaaical  continuous  or  aamplad  data 
feedback  control  systems.  In  this  papar,  charefora, 
another  approach  is  proposed.  First,  the  problem  of 
manipulator  ssrvo  control  Is  separatad  from  the  cask  con¬ 
trol  algorithms.  I.e.,  ths  manipulator  together  with  its 
servo  controllers  arc  considared  as  a  device  external  to 
tha  control  compucar  which  inceraccivsly  controls  tha 
axacution  of  s  variety  of  manipulator  casks.  Furthermore, 
Che  manipulator  control  tacks  ara  regarded  hara  as  a  sac  of 
locsrconnaccad  actions  which  can  be  mora  or  lass  compiex 
and  which  ara  executed  saquancislly  or  simulcanaously. 

In  order  to  synchssizs  tha  control  of  intarconnsetad 
actions,  chras  action  cacagoriss,  primitive,  composite  end 
complex  actions  hava  bean  Introducad.  Ths  primitive 
acciona  Include  alamantary  motions  such  ac  one-scap  shifts 
of  tha  mechanical  hand  in  a  given  coordinacs  system  and 
single  incramancs  of  spacing  or  closing  tha  end  affaccor. 
Composite  actions  ara  composed  of  scvaral  primlciva  actions 
which  sre  axacuced  saquanclally.  Examplas  ara:  alignment 
of  Che  and  af factor  to  a  givan  surface,  controlling  tha 
distance  of  the  and  affaccor  from  a  givan  surfaca  or 
objacc,  cantering  on  an  objacc,  act.  Complex  actions 
consist  of  composica  actions  chat  arc  exacucad  sequen¬ 
tially  and/or  In  parallal.  Examplas  sre:  searching  for 
an  objacc,  following  a  moving  object,  capturing  an  object. 


transferlng  an  objecc  from  one  place  co  another,  etc.  The 
paper  presents  a  separate  control  software  Implementation 
methodology  for  each  of  the  three  action  categories. 

Due  to  the  Inherent  parallellam  of  manipulator  con> 
trol  actions  In  real-time  applications,  the  .MCS  software 
outlined  In  this  paper  has  bacn  designed  as  a  concurrent 
programming  system.  Tha  concept  of  monitors,  Introduced 
by  Brlnch  Hansen  (Rafs.  13-14)  and  Hoerc  (Ref.  IS),  has 
been  employed  ee  the  main  structuring  tool  for  real-time 
control  software  development. 

For  deaonstreclon  end  evaluation,  the  real-time  soft¬ 
ware  has  bean  coded  for  and  applied  to  the  CURV  Arm  Control 
System  (CACS)  In  the  JPL  telaoparator  laboratory.  The 
first  phase  of  the  CACS  demonstrstlon  snd  performance  eval¬ 
uation  project  Is  limited  to  relatively  almple  teaks: 
interactive  searching,  following,  graaplng,  and  stopping  of 
slov.iy  moving  heevy  objects  using  date  from  proximity  and 
for^'^-torque  sensors  integrated  with  the  JPL  CURV  arm.  The 
initial  design  of  the  CACS  software  based  on  a  structured 
approach  employing  tha  monitor  concept  in  Its  simplest  form 
is  given  in  Ret.  16. 

The  CACS  software  la  an  exploratory  developisenc  aimed 
to  identify  problem  areas  and  to  experiment  with  alterna¬ 
tive  techniques  and  approaches  rather  than  to  define  a 
final  design  solution  for  interactive  and  sensor  refer¬ 
enced  manipulator  control. 

2.  MANIPULATOR  CONTROL  SYSTEM  DESCRIPTION 


Information  is  displayed  by  appropriate  monitors.  Intorma- 
tlon  from  proximity,  force-torque  end  other  sensors  can  be 
displayed  by  computer  driven  graphic  displays,  using 
context-dependent  formats  suited  to  a  particular  class  of 
tasks.  Alarm  information  can  be  displayed  by  labeled  light 
mosaic,  sound  signals,  etc. 

The  coas&and  Interface  Is  a  device  that  convert  opera¬ 
tor  cotnmands  to  computer  reedable  form.  The  commands  can 
be  in  the  form  of  a  command  language  with  e  given  syntax 
adjusted  to  a  particular  class  of  tasks.  Commands  can  be 
Issued  manually  by  TTY  or  orally  by  a  voice  command  sys¬ 
tem.  The  operator  cotnmands  can  also  be  generated  In 
discrete  form  by  switches  end  pushbuttons,  and  in  analog 
form  by  Joysticks  or  potentiometers. 

The  control  computer  is  the  central  component  of  the 
MCS  that  generates  set-points  for  menipuletor  joints  on  the 
basis  of  operator  commands  and  sensory  feedback  date  pro¬ 
cessed  by  tha  MCS  software.  The  MCS  software  performs  also 
parameter  management . 

There  ara  two  groups  of  MCS  parameters:  the  system 
and  the  cask  related  parameters.  The  first  group  contains 
all  parameters  chat  describe  the  manipulator  hardware  end 
the  conacanca  used  in  the  control  algorithms.  The  second 
group  of  parameters  is  related  to  the  manipulator  environ¬ 
ment  and  to  the  particular  cask  co  be  performed.  Both 
parameters  are  entered  via  the  computer  console;  the  first 
group  during  system  Installation,  the  second  group  Just 
before  cask  execution. 


2.1  Cenaral  Configuration  of  the  MCS 


2.2  Specific  Configuration 


In  ganaral  the  MCS  can  ba  regarded  aa  en  Incarfaca 
baewaan  tha  human  oparacor  and  tha  manipulated  obJecc. 

This  incarfaca  supports  Incaracclva  axacutlon  of  a  glvan 
class  of  tasks  ralacad  co  a  glvan  class  of  objects  and 
chair  anvlronmanc.  As  shown  In  Fig.  1.,'cha  components  of 
tha  MCS  ara:  manipulator,  information  display  devices, 
cooaand  Interface,  and  control  computer. 

Tha  manipulator  Itself  Is  a  feedback  control  system, 
usually  Implemanced  In  hardware  containing  e  mulclllnkage 
mechaalam,  end-effector  (EE),  actuators,  local  feedback 
controllara,  sensors,  sensor  support  electronics,  A/D  and 
0/A  converters. 


The  specific  Interactive  control  system  hee  been 
Implemented  for  the  JPL/CURV  linkage  arm.  The  control 
system  Is  labeled  CACS  (Curv  Arm  Control  System).  The  erm 
has  Sevan  degrees  of  freedom:  three  for  the  arm,  three  for 
wrist  and  one  for  EE.  The  mech'enlcel  details  of  the  manip¬ 
ulator  are  given  In  Ref.  17.  The  EE  le  a  parallel  Jaw 
device  with  proximity  sensors  integrated  with  It.  The 
hydraulic  actuators,  servo  controllers  and  sensor  support¬ 
ing  electronics  are  described  In  Ref.  IB.  The  complete 
spatial  position  of  the  manipulator  is  defined  by  the  fol¬ 
lowing  seven  Joint  variables: 

-  arm  azimuth 


Information  displays  convert  the  video,  sensory  and 
alarm  data  into  forms  convenient  for  the  human  operator  In 
order  to  fecllltate  the  interactive  control  process.  Video 


9^  *  elevation 
9j  -  arm  extension 


1.  Oenaral  Configuration  of  MCS 


3,  -  wrist  yaw  angle 

-  wrisc  pitch  angle 

-  wrist  roll  angle 

-  jaw  opening. 

The  manipulator  is  also  equipped  with  force-torque  sensors. 
(See  Ref.  17.)  Other  types  of  sensors  have  also  been 
developed,  for  example  touch  and  slippage  sensors  (see 
Ref.  19),  but  they  are  not  used  In  the  first  phase  of  the 
CACS  project* 


Grasping  and  stopping  an  object  must  be  performed  so 
that  once  the  object  Is  grasped  the  notion  of  the  ££  is 
referenced  to  the  force-torque  sensors  assuring  a  gentle, 
"soft"  scop. 

Transferring  an  object  means  to  change  the  location 
and  orientation  of  the  object  in  the  work  space.  This 
action  can  be  a  part  of  an  assemblv/dlsassembly  task. 

The  goal  of  object  mapping  is  to  create  a  topographic 
profile  of  a  partially  unknown  object  or  of  a  terrain  in 
the  work  space.  Mapping  Is  based  on  proximity  sensoi 
measurements . 


Proximity  sensors,  four  of  them,  are  integrated  with 
the  two  jews  of  the  ££,  one  pair  pointing  In  forward  direc¬ 
tion,  the  other  peir  pointing  perpendicular  to  the  first 
one,  downward  the  ££.  The  sensing  range  of  the  proximity 
sensors  Is  about  three  inches.  The  force-torque  sensor  is 
mounted  to  the  base  of  the  ££.  The  sensor  measures  the 
three  perpendicular  forces  and  the  three  perpendicular 
torquea  referenced  to  the  base  of  the  ££.  The  dynamic 
range  of  the  sensor  la  0.5  to  300  M. 

The  information  display  system  contains  TV  monitors 
and  a  microprocessor-driven  color  graphic  display.  (Ref. 
20.)  The  alarm  display  is  temporarily  Implemented  on  the 
control  computer  console. 

The  analog  commands  are  generated  by  two  3-dimensional 
joysticks,  and  by  a  potentiometer.  The  discrete  operator 
comnands  are  generated  by  a  set  of  switches,  ell  incorpo¬ 
rated  into  a  multipurpose  device  called  Universal  Control 
Panel. 

The  control  computer  is  an  Interdate  m70  (or  8/16E) 
minicomputer  (Ref.  18). 


Using  the  complex  actions  listed  above  a  large  number 
of  manipulator  tasks  can  be  accomplished.  Including  simple 
assembly  and  disassembly  operations,  that  are  assisted  by 
the  operator  In  an  interactive  manner.  The  list  could  also 
be  extended  by  adding  programmed  actions  to  the  control 
"menu."  These  actions  would  be  performed  by  executing 
fixed  stored  programs  loaded  from  the  MCS  library.  Pro¬ 
grammed  fixed  actions  are  not  considered  In  this  paper. 


3.2  Composite  actions 


In  order  to  perform  a  complex  action  the  control  sys¬ 
tem  will  enforce  the  execution  of  a  set  of  appropriate 
composite  actions  that  are  less  complex.  Execution  of 
composite  actions  can  be  In  series.  In  parallel,  or  com¬ 
bined,  depending  on  the  specific  complex  action  commanded 
by  the  operator.  Examples  of  composite  actions  are: 


•  manual  control 


•  roll  alignment 

•  pitch  alignment 


3.  ^iAH^PULATOR  ACTIONS 


yaw  alignment 


Accomplishment  of  a  given  manipulation  task  can  be 
considered  as  the  execution  of  a  set  of  actions  invoked 
explicitly  by  operator  commands,  or  implicitly  by  control 
algorithms  that  are  implemented  in  the  MCS  software. 
According  to  their  complexity,  the  actions  can  be  separated 
into  three  groups:  complex,  composite  end  primitive 
actions.  These  groups  will  now  be  discussed  separetely. 


3. 1  Complex  actions 


•  elevation  control 

e  distance  control 

e  speed  identification 

•  centering 

e  grasping 

•  tracking 


Complex  actions  will  be  defined  as  elements  of  a 
manipulator  tesk  that  are  sufficiently  general  and  have 
unique  physical  and  logical  cheracceristics.  In  addition, 
it  will  be  assumed  that  these  actions  can  only  be  initiated 
by  the  operator.  Examples  of  complex  actions  are: 

e  searching  for  object 

e  following  the  moving  object 

•  grasping  end  stopping  the  object 

e  transferring  the  object 

•  mapping  the  object. 

Searching  for  an  object  means  to  bring  the  EE  to  the 
vicinity  of  the  object  measured  by  proximity  sensors.  The 
control  of  this  action  can  be  performed  by  the  operator 
manually,  or  It  can  be  performed  automatically  using  a 
search  algorithm  the  parameters  of  which  are  specified  by 
the  operator.  Also,  the  search  can  be  unconstrained  in 
space,  or  constrained  to  a  fixed  surface. 

Following  a  moving  object  meana  to  movs  the  EE  so  that 
a  constant  distance  is  being  kept  between  the  EE  and  the 
object.  This  action  has  also  manual  end  automatic,  uncon¬ 
strained  end  constrained  options.  In  addition,  the  object 
following  action  can  be  referenced  to  the  front  or  to  the 
lower  proximity  sensors. 


e  stopping. 

Using  manual  control  the  operator  controls  the  overall 
motion  of  the  manipulator,  affecting  directly  or  Indirectly 
all  joint  coordinetes  via  analog  commands.  The  manual  com¬ 
mands  are  resolved  into  appropriate  manipulator  joint  com¬ 
mands  through  the  MCS  software.  Manual  control  can  be 
unconstrained  or  constrained  in  space.  The  coordinate 
system  corresponding  to  analog  counands  can  be  chosen  by 
the  operator.  Different  coordioeta  systems  that  can  be 
used  will  be  discussed  in  the  next  section.  Manual  control 
can  have  two  modes:  position  and  rate  control  mode.  In 
position  mode  the  manipulator  control  system  executes 
motion  increments  defined  by  analog  commands  In  the  sense 
of  changing  positions  of  the  EE.  In  rate  mode,  the  manipu¬ 
lator  control  system  will  Interpret  the  analog  commends  as 
reference  inputs  for  the  directional  speed  of  the  manipu¬ 
lator  E£  in  a  given  coordinate  system. 

Alignment  operations  are  referred  to  the  equalization 
of  the  lengths  of  proximity  sensor  beame  relative  to  the 
alignment  surface.  The  alignment  surface  can  be  en  object 
or  a  tracking  surface.  In  roll  alignment,  the  lower  prox¬ 
imity  sensors  must  Indicate  equal  lengths.  In  pitch  align¬ 
ment  the  lower  proximity  sensors  must  have  equal  length  in 
several  successive  longitudinal  positions  of  the  EE.  In 
yaw  alignment,  the  front  proximity  seneor  beams  must  indi¬ 
cate  equal  length#  relative  to  the  alignment  surface.  The 
condition  for  starting  an  alignment  action  Is  to  bring  the 
EE  in  proximity  of  the  alignment  surface.  Alignment  opere- 
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cions  can  be  performed  relative  co  static  or  dynamic  align* 
ment  surfaces.  A  static  surface  has  a  fixed  position  and 
shepe  in  Che  work  space.  A  dynamic  surface  is  slowly 
moving  or  changing  its  profile  in  che  work  s:>ace.  In  all 
alignment  operations  it  is  assunved  chat  che  alignment  sur* 
face  Is  sufficiently  smooch  so  chat  it  can  be  approximated 
by  a  tangent  plene  in  the  vicinity  of  che  footprints  of  che 
proximity  sensor  beams  on  the  surface. 

Elevation  and  distance  control  are  also  referenced  to 
proximity  sensors.  In  elevation  control  che  beam  lengths 
of  the  lower  proximity  sensors  must  be  maintained  conatant 
and  equal  to  a  given  value.  In  distance  control  the  beam 
lengths  of  che  front  proximity  sensors  are  maintained  con* 
stant  and  equal  to  a  given  value.  Elevation  and  distance 
control  can  also  be  referenced  to  static  or  dynamic 
surfaces . 

In  speed  identification  che  speed  of  che  EE  must  be 
equelized  to  che  speed  of  che  moving  object  or  moving 
crocking  surface.  Speed  identification  must  be  accom* 
plished  before  starting  an  alignment,  elevation  or  distance 
control  operation. 

Centering  means  co  adjust  che  opening  and  che  lateral 
position  of  che  EE  relative  co  a  given  object  before  grasp. 
Centering  can  be  performed  on  both  static  and  dynamic 
objects.  In  che  case  of  dynamic  objects,  centering  also 
contains  a  dynamic  alignment  operation  of  che  EE  relative 
to  che  object.  It  means  that  che  EE  must  get  in  che  call 
position  to  che  object  so  chat  che  motion  of  che  EE  is 
parallel  to  che  longitudinal  axis  of  che  EE.  This  is  che 
best  grasping  position. 

Grasping  an  object  consists  of  a  short  programmed 
sequence  of  EE  operations,  with  che  final  result  of  having 
and  contacting  Che  object  within  che  claws  of  che  EE.  Once 
this  occurred,  che  motion  reference  is  changed  from  proxim* 
icy  CO  force*corque  sensors. 

Tracking  an  object  means  a  joint  motion  of  che  EE  and 
che  greaped  object  so  thee  che  manipulator  automatically 
complies  with  che  forces  of  che  object  exerted  on  che  EE. 
This  eccion  is  foree*corque  sensor  referenced  in  che  sense 
chat  che  reaction  forces  exerted  by  che  object  on  che  EE 
must  be  maintained  at  e  specified  minimum  level. 

Stopping  an  object  means  to  stop  che  motion  of  che 
manipulator  gradually,  keeping  che  reaction  forces  exerted 
by  che  object  on  che  EE  and  measured  by  che  force*corque 
sensor  below  some  value  specified  by  che  operator. 

The  given  list  of  composite  actions  is  not  complete. 

It  ceo  be  exceoded  by  ocher  actions,  for  example,  edge 
cracking,  scanning,  etc.  « 

3.3  Primitive  actions 

Completion  of  a  complex  action  is  accomplished  by  che 
sequential  execution  of  a  series  of  motion  increments  of 
che  manipulator.  Execution  of  a  motion  increment  will  be 
called  a  primitive  action  of  the  manipulator.  In  this  work 
two  groups  of  primitive  actions  are  considered: 
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Fig.  2,  Representation  of  Coordinate  Systems 
WCS.  res,  and  HCS 

WeS  is  a  Cartesian  coordinate  system  fixed  to  the 
manipulator  environment,  i.e.  co  che  world  in  which  che 
manipulator  is  acting.  In  this  work  che  origin  of  WCS  is 
fixed  to  che  base  of  che  manipulator.  The  point  in  WCS  is 
a  vector  qy  ■  (x,  o,  j) ,  where  x  ■  (x,  y,  r)  is  position  of 

Che  wrist  of  che  manipulator,  o  -  (o,  3,  y)  Is  che  angular 
position  of  the  hand  and  u  is  che  opening  of  che  EE. 

TCS  represents  a  two  dimensional  space  defined  by  che 
fixed  plane  TP.  The  orientation  of  this  plane  in  world 
space  is  defined  by  the  vector  o  *  Because  only 

motion  increments  are  considered  here,  che  origin  of  thee 
plane  can  be  Ignored.  When  che  EE  lies  in  a  plane  parallel 
CO  che  tangent  plane  TP  of  che  cracking  surface  TS,  i.e.  if 
its  roll  end  pitch  axes  are  aligned  with  che  TP,  then  che 
position  of  che  EE  is  defined  by  q^  ■  (^,  .,) ,  where 

x^  ■  (x.j,  y^)  ia  a  point  in  che  TP. 

HCS  is  also  e  Cartesian  coordinate  system  defined  by 
three  orthogonal  axes  fixed  co  che  EE.  The  C*axl8  is  che 
longitudinal  axis  of  che  EE  (it  is  the  roll  axis  of  che 
EE)  and  defines  che  '*forward*backward”  direction;  che 
*i*axis  is  che  lateral  axis  of  the  EE  and  defines  che 
"left*rlghc”  direction;  and  che  ;*axls  is  che  orthogonal 
complement  of  che  i  end  h  axes  and  defines  the  "'jp*dovn'' 
direction.  A  point  in  the  HCS  is  defined  by  a  vector 
Sh  *  ^1*  -H’  where  £  -  (4.  n,  ;)  and  •  (i^,  3^,  y^^). 

The  HCS  plays  an  important  role  in  almost  ell  composite 
actions  referenced  co  proximity  sensor  measurements. 

The  unconditional  shifts  can  be  defined  es  increments 
in  the  four  coordinate  systems  described  above: 

-  A0  in  JCS 


e  unconditional  increments  (shifts) 

e  condlcionel  increments  (shifts) 


&q^j  ■  ^w)  in  WCS 

-  (A^,  Ao,  Aw)  in  TCS 


(1) 


Both  groups  have  two  options:  position  and  race  con* 
crol  mode.  Primitive  actions  can  be  referred  co  four  dif* 
ferent  coordinece  systems:  joint  coordinate  system  (JCS), 
world  coordinate  system  (WCS),  cracking  plene  coordinate 
system  (TCS)  and  hand  coordinate  system  (HCS).  The  four 
reference  systems  ere  shown  in  Fig.  2. 


3o,  Aw)  in  HCS 


where  ell  components  of  che  vectors  (j  -  W,T,H)  cen  be 

chenged  independently.  Aa  indicated  in  expressions  (1). 
che  increments  of  hand  angles  end  che  EE  opening  ere 


invariant  in  ell  Cartesian  coordinate  systems. 


The  point  in  JCS  is  defined  by  the  vector  £  •  (0^, 
02....,  0j^),  where  0^  are  manlpuletor  joint  veriables 

alreedy  described  in  subsection  2.2.  It  is  noted  that  che 
0^  positions  of  che  manlpuletor  ere  measured  by  potentioo* 

eters  ec  each  joint. 


Conditional  shifts  imply  conditions  chat  introduce 
some  interdependence  between  che  components  of  Aq^.  Here 

we  only  consider  conditional  shifts  in  the  HCS,  with 
condiclona  related  to  che  proximity  sensor  beams.  Four 
examples  are  shown  in  Fig.  3.  First  a  conditional  roll 


increment  where  che  reflection  point  of  Che  lower*lefc 
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aonredundanc ,  chis  mapping  is  anlque  with  a  unique  Inver* 
Sion  £  ^  *  Combining  (2),  (3)  and  (4)  we  have: 

i„  «3t,.  -  r’  (-3-J  ^  I  -  io  • 

It  should  be  noted  that  the  calculation  of  requires  the 
solution  of  two  sets  of  manipulator  equations,  £  and  its 

inverse  £  \  This  Is  so  because  onl/  che  positions  In  the 
JCS  are  measurable. 


In  the  case  of  TCS,  the  JCS/WCS  mappings  are  formally 
as  before 

-T  -o^  ■  \  -o^  • 

where  Aq^  *  (Asc* ,  Aa,  0,  0,  Au)  is  an  Incermedlace 
Increment  defined  by  a  simple  coordinate  transformation 


"Ax  ' 

ix'  -  •••  ' 

[  0 


Fig.  3.  Conditional  Shifts 

proximity  sensor  is  kept  fixed.  As  shown,  the  EE  changes 
Its  roll  angle  Ay  without  moving  the  reflection  point  of 
the  lower  left  sensor,  and  keeping  the  length  of  that 
sensor  beam  unchanged.  Similarly,  che  conditional  pitch 
and  yaw  Increments  are  performed  with  fixed  reflection 
points  of  the  lower*rlghc  and  frooc*rlghc  proximity  sensor 
beams,  respectively,  the  last  example  shows  a  conditional 
EE  opening  where  che  position  of  che  right  finger  of  the 
EE  Is  kept  fixed. 

Position  and  race  control  modes  are  options  chat  can 
be  selected  by  the  operator  or  by  che  control  algorithms. 
Position  mode  corresponds  to  the  regular  execution  of  an 
Increment  Aq^.  In  che  rate  mode  the  given  Increment  Aq^  will 

be  executed  In  a  repetitive  manner  In  order  to  generate 
Che  speed  of  che  EE  chat  Is  proportional  to  the  increment 
V  •  Aq/At,  where  At  Is  che  time  interval  between  two 
consecutive  Increments. 

Primitive  actions  are  Invoked  aucomeclcelly  by  the 
control  algorithms  which  Implement  che  composite  actions. 

Implementation  of  primitive,  composite  and  complex 
actions  are  discussed  In  che  subeequent  three  sectlona. 


with  £  being  a  3x3  rotation  matrix  chat  corresponds  to  che 
angular  position  of  che  cracking  plane  TP  In  che  WCS. 

Similarly,  for  Increments  HCS  we  can  write: 

♦  (Aq  6  )  •  ♦  (Ac’*.  6  )  .  (8) 

-o'  -W'^  3w*  -o'  *  ^  ' 

where  Aq*y  ■  (Ax"«  Au)  and 

Ax'*  -  R(aj5)A£  .  (9) 

Vector  In  (9)  corresponds  to  che  temporary  orientation  of 

the  hand  In  the  WCS.  As  seen,  che  transformation  from  the 
HCS  to  che  WCS  Is  based  on  a  temporary  hand  orientation 
which  is  measured  Indirectly  via  the  JCS.  Therefore,  che 

current  value  of  o  must  be  calculated 
-0 

Where  £  represents  a  subset  of  the  manipulator  geometry 
aquatlona  (A) . 

Conditional  shifts  ere  more  complex.  To  give  an  Idea 
of  this,  consider  only  che  first  and  Issc  examples  shown 
In  fig.  3.  It  can  be  shown  that  for  a  given  Ay,  two 
additional  shifts  must  be  executed  In  che  HCS  In  order  to 
keep  che  reflection  point  of  cha  lower-left  proximity 
sensor  fixed: 


A.  ItflPLEMEKTATION  OF  PRUIITIVE  ACTIONS 
A.l  Coordinate  transformations 


(1  *  cos  Ay)  (S  +  S  )  sin  A> 
0 


The  execution  of  e  primitive  ectlon  means  to  move  the 
menlpulator  from  Its  current  position  9  In  JCS  to  a  new 
position 

9  -  9  +  i9  ,  (2) 

—  — 0  — 

that  corresponds  to  a  desired  Increment  defined  In  e 

selected  coordinate  system  J.  Thet  is,  for  e  given  and 

9^  the  follovlng  coordinate  transformation  must  be 
performed: 


*^9  ■ 


J  -  W.T.H. 


For  che  WCS  one  hes  to  consider  the  equeclons  of  menlpule- 
cor  geometry  In  che  general  form: 


fin  Ay  -  (S  S^)  (1  -  cos  Ay) 


where  S  ■  S,,,  w  Is  che  EE  opening,  S  end  w  arc  fixed 
Ll.  0  0 

peraaecers  of  che  manlpulecor.  Similar  equeclons  hold  for 
condltlonel  pitch  and  Jaw  Increments.  The  conditional  EE 
operation  Is  much  simpler  and  require  only  laterel 
corrections: 


IY"  If  right  finger  is  fixed 

A(j 

-  J-  If  left  finger  Is  fixed. 

4.2  Llneerlretlon 


where  F  It  e  nonlinear  analytic  vector  function  thet  maps  a 
point  In  JCS  onto  a  point  In  WCS.  If  the  menlpulator  la 


As  sacn,  all  coordlnatt  transformations  are  baetd  on 
tha  function  by.  This  can  erttrt  a  httvy  computational 

burdan.  Thertfort,  tha  llnctrltatlon  of  is  Introducad. 
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I’c  is  easy  co  show  thac  for  sufflclencly  small 
equaclons  (3)  and  (5)  can  be  replaced  by: 

l/here  ^  (6  )  Is  Che  Inverse  Jacobian  of  F  (6  ): 

—  — o  ”  — o 

Llnearlzaclon  simplifies  Che  compucacions  considerably,  buc 
incroduces  cummulacive  errors.  Forcunacely,  as  seen  lacer, 
Che  prlmiclve  accions  are  Imbedded  inco  iceracive  feedback 
loops  so  chac  cuiznnulacive  error  cannoc  occur. 

4 . 3  Decoupling  Che  Joinc  coordinaces 

The  cofflplexicy  of  che  mapping  (4)  depends  on  che  geo- 
mecric  conscruccion  of  che  manlpulacor.  If  che  manipulator 
geomecry  provides  only  a  loose  kinemacic  coupling  between 
arm,  wrlsc  and  EE  coordinaces,  che  joinc  veccor  d  can  be 

considered  as  a  composicion  of  chree  vectors  6.,  d,,  and 

“A  “H 

6^.  The  chree  veccors  represenc  che  arm,  che  wrlsc  and 

che  EE,  respeccively ,  and  cheir  mucual  dependence  is 
reduced  co  a  simple  addiclon  of  angles.  Due  co  che  double 
parallelogram  nacura  of  che  JPL  CURV  arm,  che  orien- 
carion  dependence  between  and  only  requires  a  simple 

correcrion  of  6^  for  every  change  in  6^.  As  a  consequence. 

che  7«7  matrix  a(6  )  can  be  reduced  co  two  3x3  macrlces 

$  (6  ),  and  ^(,(6  ),  and  che  equation  (13)  becomes: 

—A  *-o  -o 

. 

v.h.r.  h  ’  «7* 

ie^^rr  ■  kg  is  a  scaling  factor  chac  con¬ 


verts  EE  opening  into  a  corresponding  Joinc  angle.  The 

matrix  >.(6  )  for  che  JPL  CURV  arm  has  a  relatively  simple 
“A  ~o 

form  (Ref.  16).  The  matrix  ^„(6  )  is  a  unit  matrix  for  che. 
JPL  CURV  am.  -H  -o 

The  coordinate  cransf ornacions  in  che  CACS  can  be 
summarized  as  shown  by  a  data  flow  diagram  in  Fig.  4. 

This  diagram  contains  four  kinds  of  graphic  svmbols: 
arrows  representing  veccor  or  scalar  data;  square  boxes 
representing  computation  of  a  function;  arithmetic  symbols 
and  oval  boxes  representing  che  data  buffers.  Data  buffers 
play  an  important  role  in  che  approach  described  in  this 
paper.  They  will  not  be  discussed  in  detail  here,  and 
will  only  be  considered  as  memory  locations  allocated  co 
che  shared  data  chac  are  used  in  different  compucacions. 

The  pare  of  Fig.  4  fenced  by  docced  lines  represents  che 
coordinace  cransformacions  from  TCS  and  HCS  co  che  WCS. 
Therefore,  UCS  is  che  common  coordinate  system  for  all 
primicive  accions.  The  inpuc  incremencs  on  che  lefc  side 
of  Fig.  4  are  generated  by  various  control  algorithms 
which  implement  che  execution  of  composite  accions.  The 
results  of  che  coordinace  cransformacions  are  accumulated 
in  che  world-space  buffer.  This  is  a  transit  buffer  in 
che  system,  with  che  purpose  co  collect  incremencs  from 
different  control  algorithms  before  che  final  VCS  co  JCS 
mapping  is  performed. 

4.4  Superposition  of  che  control  modes 

As  discussed  previously,  some  composed  accions  are 
related  co  moving  objects  or  moving  cracking  surfaces. 

This  means  chac  positional  shifts,  generated  by  control 
algorithms,  are  relative  co  che  moving  object  or  cracking 
surface.  As  an  example,  Fig.  3  illustrates  che  case  where 
che  EE  is  following  an  object.  The  speed  of  object  v^  and 

che  speed  of  che  equal,  buc  che  position  of  che 

EE  relative  to  che  object  must  be  corrected  by  a  lateral 
shift  -^n.  This  shift  muse  be  added  co  che  corresponding 
race  increment  v^it. 

In  this  work  such  additions  are  handled  within  che 
frame  of  che  UCS.  Therefore,  two  parallel  versions  of 
world-epace  buffets  are  introduced,  che  posicion  and  che 
race  world-space  buffer.  As  shown  in  Fig.  6,  che  posicion 
buffer  colleccs  all  positional  shifts  ^q  generated  by  che 
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FIk.  4.  Coordinate  Transformations  In  CACS 


rig.  Example  of  Combined  Motion 


^SfTlON 

SUFFER 


<*5  TO  JS  H 

/ 

MAffINC  * 

YAWUa.CM) 
PITCH(iB,CM) 
ROLL (Ay, CM) 
MOVEK(Ae,CM) 

MOVEE(An,CM) 

MOVE2(AC,CM) 

SLIDE  (Ajtj., CM) 

CONTR(Au!,CM) 

EXPND(Aui,CM) 


Move  EE  cranslatori^  fcr  Ax  in  WCS. 
'!Hand  angles  unchanged.) 

Rotate  hand  for  angle  lo 

Rotate  hand  cor  angle  A3 

Rotate  hand  for  angle  Ay 

Move  EE  translatory  for  AE  in  HCS 
(forward  -  backward) 

Move  EE  translatory  for  An  in  HCS 
Cleft  -  right) 

Move  EE  translatory  for  A;  in  HCS 
upward  -  downward 

Move  EE  translatory  for  in  TCS 
Contract  Che  EE  for  Aw 
Expand  the  EE  for  Aw 


fig.  6.  Superposition  of  Control  Modes 


control  algorithms.  Vhenever  an  increment  is  sent  to  the 
buffer  it  is  added  to  the  actual  content  of  the  buffer. 

This  kind  of  buffer  access  is  marked  by  an  addition  symbol 
on  Che  diagram.  In  the  time  of  WCS  to  JCS  mapping,  the 
accumulated  increment  ■j^pQ^t^chat  represents  the  position 

contribution  co  the  total  increment  A^,  must  be  cransfered 

CO  Che  mapping  routines.  The  transfer  procedure  must  reset 
Che  buffer  after  receiving  its  content.  Therefore,  the 
buffer  will  be  empty  by  the  time  when  the  next  increment 
is  executed «  provided  chat  it  was  not  filled  again  by  the 
control  algorithms.  Buffer  reseclng  is  marked  by  "R**  on 
the  diagram.  The  race  buffer  concalnes  Che  race  Increment 
chat  corresponds  to  spad  v,  and  there  is  no  addition 

property  Involved  in  the  corresponding  accessing  proce-* 
durea.  Also*  the  procedure  chat  reads  the  buffer  does  not 
reset  the  buffer.  Consequently  the  race  contribution 
A^^^^  exists  in  every  iteration  cycle  of  the  motion 

Incremeoc  execution  until  its  content  is  nullified  by  the 
control  algorithms. 


4.5  Motion  primitives 


The  coordinate  tranaformatlons  contained  in  the  dotted 
area  of  the  Fig.  4.  are  implemented  in  software  as  a  sat  of 
subroutines  chat  will  be  called  "motion  primitives.*  The 
list  of  motion  primitives  employed  in  this  work  is  given 
in  Table  I. 

Motion  primitives  arc  subroutines  called  in  the  pro« 
grams  chat  implement  the  execution  of  composite  actions. 

All  motion  primitives  have  only  input  parameters.  These 
parameters  define  motion  increments  In  a  given  coordinate 
system,  the  control  mode  and  the  additional  condition  data 
in  Che  case  of  conditional  shifts.  The  result  of  the 
coordinate  cransfonDacions  is  placed  into  the  world-space 
buffer,  position  or  race  version,  depending  on  the  control 
mods  Idcncifiar. 


5.  IHPIXMENTATION  OF  COMPOSITE  ACTIONS 

As  discussed  before,  execution  of  a  composite  action 
consists  of  s  sequential  execution  of  primitive  actions. 
Thertfore,  cha  problem  of  Implementing  composite  actions  is 
Che  prob)am  of  generating  a  sequence  of  motion  increments. 


YAWC(FP,S,w,Ao,CM)  Rotate  hand  conditionally  for 
angle  Ao 

PITCHC(FP,S,w, AB.CM)  Rotate  hand  conditionally  for 
angle  AB 

ROLLC(FP,S,w,Ay,CM)  Rotate  hand  conditionally  for 
angle  Ay 


CONTRC(FP,Aw,C.M> 


EXPMDC CrP,Aw.CM) 


Contract  che  EE  conditionally 
for  Au 

Expand  Che  EE  condi Cionally  for  Aw 


Notices:  C.M  is  control  mode:  CM*0  for  position,  CM»1 

for  rate.  F?  is  sensor  identifier:  FP-0  for 
left,  FP»1  for  right  proximity  sensor. 


5. 1  The  controller  and  Its  state  equation 

(k) 

Generation  of  a  sequence  of  moclor.  increments  Aq 
(the  coordinate  system  will  be  ignored  here)  can  be 
described  by  the  following  iterative  equation: 

) ,  k-0,),2,...  (16) 

(k)  fk'i 

where  k  denotes  the  Iteration  cycle,  while  and  C  ' 

are  vectors  of  sensor  data  and  human  operator  commands 

fk) 

taken  in  the  k-th  iteration  cycle.  The  value  has 

been  measured  when  che  manipulator  was  In  che  position 
(k) 

9^'  .  The  vector  function  ^  represents  the  control  algo¬ 

rithms  which  generally  involve  both  numeric  calculations 
and  logic  decisions.  These  algorithms  will  be  called  here 
"controllers'*.  A  controller  is  allocated  co  each  composite 
action.  (These  controllers  should  not  be  misinterpreted  as 
local  feedback  controllers  in  che  manipulator  servo  hard* 
ware.  They  have  nothing  in  common.) 

Because  che  iterative  equation  (lb)  involves  'ini'  )re 
Iteration  cycle,  ^  is  a  meraoryless  wontrollet.  '*nf »rtun- 
scaly,  the  majority  o.'  composite  actions  are  not  »  ei-r  • 
and  cannot  be  Implemanced  by  memor/Iess  'ontrolXert 
control  algorithms  may  require  the  data  from  more,  ta 
last  M  iteration  cycles.  Thus,  che  list  of  ar(j»«r  * 

should  be  extended  by  *' 

M-1.  A  more  elegant  way  to  deal  vicn  ii  ««• 
introduce  state  variables.  State  virlsh^^- 
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benefit  of  the  recurrence  idee,  and  considerably  simplify 
Che  fonaulacion  of  che  control  algorithms  and  the  corres* 
ponding  memory  operations. 


According  to  Fig.  3.  che  pitch  increment  will  be: 


The  state  variables  are  suitably  chosen  data  chat  are 
calculated  additionally  in  each  iteration  cycle  and  memo¬ 
rized  for  use  in  che  next  iteration  cycle.  The  state 
variables  can  be  grouped  into  a  state  vector.  The  dimen¬ 
sion  of  che  state  vector  corresponds  to  che  number  of 
iterations  contained  in  che  iterative  equation.  Since  che 
control  algorithms  contain  many  logic  variables  and  logic 
decisions,  che  state  variables  for  che  controller  can  also 
be  boolean  variables.  Therefore,  two  state  vectors  will 
be  introduced  here:  a  numeric  state  vector,  r^  >  (r^*r2t 

...,rj^)  whose  componenta  r^  are  numeric  variables,  and  a 
logic  state  vector  •  •  •  •  •  whose  components 

are  boolean  variables.  The  controller  with  memory  can  now 


-  arctg 


y  "^cesc 


(k)  (k-1) 

where  and  are  lengths  of  lower  proxiraitv  sen¬ 

sors  (one  of  them)  in  two  succeslve  iterations  when  che 
test  shift  has  been  applied.  The  algorithm  chat 

implements  the  pitch  alignment  will  be: 

if  i  •  false  then 


be  described 

by  the 

following  recurrence  relation: 

A8;»0; 

- 

j(k) 

3(k)_ 

c(k) 

Z:*crue 

j(k) 

c(k) 

.  .  k-l.’,. 

.{k)_ 

jfk) 

jk) 

(17) 

end 

,(0)  . 

else 

begin 

JO)  . 

z  . 

AC:-0; 

where  r  and 
*0 

Z  are 

•0 

inltl.l 

values  of 

State  vectors  £ 

and 

and  ^  and  h  are  numeric  and  boolean  vector  functions, 
respectively. 


The  iterative  process  (17)  Is  illustrated  in  Fig.  7. 

AS  indicated  in  this  figure  a  special  buffer  is  allocated 
to  che  controller  for  memorizing  che  numeric  and  logic 
state  vectors.  The  coordinate  transformations  have  already 
been  described  in  che  previous  section.  The  broken  arrow 
at  che  manipulator  input  in  Fig.  7  indicates  che  delay 

(k) 

betveen  cvo  succesive  values  of  manipulator  inputs  ^ 
and  This  delay  will  be  discussed  later. 


seN5or>r  fccoiack 

Fig.  7,  Iterative  Generation  of  Motion  Increments 

Two  simple  examples  are  considered  now:  roll  and 
pitch  alignment.  According  to  Fig.  3,  che  roll  increment 
for  full  roll  alignment  is  given  by: 


where  che  initial  conditions  are  r  •  0,  Z  •  false. 

0  o 

As  seen,  the  controller  uses  two  state  variables,  r 
and  1.  The  first  memorizes  the  preceding  value  of  the 
proximity  sensor  beam  and  the  second  is  a  boolean  variable 
chat  controls  alternation  of  test  and  correction  Incre¬ 
ments.  A  more  complex  example  is  the  centering  procedure 
that  requires  che  use  of  five  logic  state  variables. 

In  ideal  cases,  roll  and  pitch  alignments  can  be  per¬ 
formed  in  one  and  two  iterations,  respectively.  However, 
due  to  noise  in  the  sensor  signals,  errors  in  che  manipu¬ 
lator  servo  and  linearization  of  the  manipulator  geometrv 
equations,  it  can  be  expected  that  succesaful  alignment 
cannot  be  achieved  in  che  first  trial.  Therefore,  che 
alignment  process  must  be  repeated  several  times  until 
some  termination  criteria  are  satisfied.  For  practical 
reasons,  therefore,  che  corrections  Iv  and  18  should  be 
limited.  If  the  limiting  value  is  sufficiently  small, 
then  che  function  arctg  can  be  linearized.  Also,  some 
averaging  techniques  can  be  applied  to  che  sensor  data.  If 
the  general  first  order  filter  is  used,  che  state  equation 
of  che  controller  for  roll  alignment  will  become: 


This  equation  describes  a  simple  memory  less  controller. 
Pitch  alignment  can  be  performed  by  constant  longitudinal 


test  shifts  AC. 


This  is  necessary  because  che  £E  has 


only  cvo  lower  proximity  sensors.  (The  condition  to  start 
pitch  alignment  is  to  complete  first  che  roll  alignment.) 


Ay  if  y  >  iY 

max  ^  max 


tf 


where  •  sf  ,  Ay  is  the  maximum  value  of 

L  LL  LR  max 

roll  Increment  defined  as  a  MCS  paramecer,  and  a,  b  and  c 
are  filtering  constants.  It  is  noted  that  these  parameters 
do  not  depend  on  the  mechanical  characteristics  ot  the 
manipulator. 


5.2  Virtual  Manipulator 

An  importent  feature  of  the  iterative  process 
described  by  equation  (17)  is  that  the  functions  X*  &  il 
do  not  depend  on  the  dynamic  charecteristics  of  the  manipu* 
lator.  The  manipulator  is  here  considered  as  a  device 
extemel  to  the  control  computer  having  only  two  oblige- 

(ki 

cions:  to  execute  the  increment  within  an  arbitrary 

(k) 

but  finite  time  interval  At^  »  and  to  notify  the  control 

computer  when  the  execution  is  completed.  The  length  of 
(k) 

the  time  interval  At'  depends  on  the  current  dynamic  per- 

^  (k) 

formance  ot  the  manipulator,  on  the  current  position  ^ 

of  the  manipulator,  on  the  length  and  orientation  of  the 
(k) 

increment  Ai  ,  and  on  the  load  currently  being  handled 
by  the  EE.  Therefore,  the  atanlpulator  execution  intervals 
are  very  complicated  funcciona  of  many  end  partly  unknown 
parameters  exposed  to  unexpected  changes.  Even  for  the 
(k)  (k) 

same  Ad  the  execution  time  At'  can  be  different.  How- 

rt 

ever,  this  is  irrelevant  to  the  control  algorithms,  i.e. 
to  the  controller  that  executes  the  composite  action.  The 
controller  is  synchronized  so  that  after  generating  the 
(k) 

increment  A^  it  waits  until  the  manipulator  reaches  the 
new  position  •  Then,  the  controller  reads  new  values 

of  sensor  data  in  order  to  generate  a  new  increment 

Ag{k-H)^  The  manipulator  is  capable  of  executing  any 
““(k) 

Ai  due  to  its  own  local  feedback  controllers  attached 
to  all  manipulator  joints.  Because  of  this  autonomous 
capability,  the  manipulator  will  be  called  a  ''virtual 
manipulator."  The  degree  of  virtualization  can  be  lower 
or  higher,  depending  on  the  sophistication  and  computar 
support  of  Che  local  feedback  control  system.  For  example, 
the  manipulator  can  be  supported  by  dedicated  microproces¬ 
sors  to  improve  the  performance  of  the  local  feedback  con¬ 
trol.  Also,  more  sophisticated  algorithms  can  be  used  for 
Che  local  feedback  control. 


The  concept  of  a  virtual  manipulator  divorces  the 
problem  of  che  design  and  implcmentetion  of  interactive 
manipulator  task  control  algorithms  from  the  problem  of 
manipulator  local  feedback  controllers. 

The  virtual  manipulator  will  now  be  defined  more  pre¬ 
cisely.  First,  the  position  %  of  the  manipulator  is  said 
to  be  stable  if  the  following  inequalities  hold: 

1^1  -  ^  ‘Ji  ■  '  ^  'o-  ^  *  >’2 . 

where  3_.  (c)  are  eccuel  veluea  of  oanlpuletor  Joints,  t  Is 
r  i  o 

Che  else  when  observation  starts,  and  are  tolerances 

given  by  che  MCS  designer.  The  colerences  oust  be  in 
accordence  with  che  reaoluclon  of  Joints  and  sensors,  and 
with  che  requirements  of  che  percicular  oanlpulator  tesk. 
Tha  virtual  oanlpulecor  executes  che  Increment  18,  in  JCS  if 
it  moves  from  che  stable  position  to  che  stable  position 

8*9  +48  (see  Fig.  8). 

—  — O  “ 

Now,  che  following  assumption  must  be  satisfied  con¬ 
cerning  the  virtual  manlpuletor:  For  given  jj,  jj  • 

(Cjj , . . .  ,ej^) ,  end  for  any  8^  and  ^belonging  to  che 

domain  of  all  admissible  positions  of  che  menipulator,  che 
time  4C,,  of  execution  of  che  motion  increment  48-9-8 

la  finite  end  less  than  a  prescribed  velua.  The  control 


algorithms  and  the  system  synchronization  is  based  on  this 
assumption. 


»♦ 


5.3  Controller  scheduling 

The  iterative  process  shown  in  Fig.  7  must  be  orga¬ 
nized  so  that  che  executions  of  the  controller  and  the 
virtual  manipulator  are  perforreo  in  the  proper  sequence. 
I.e.,  the  controller  must  wait  until  the  manipulator 
achieves  stable  position  and  sends  sensor  data.  Also,  the 
virtual  manipulator  must  wait  for  the  controller  until  it 
generates  a  new  motion  increment.  This  organization  will 
be  called  scheduling  of  the  controller.  There  are  two 
basic  approaches  to  controller  scheduling:  the  synchronous 
and  asynchronous  approach.  Both  are  illustrated  in  Fig.  9. 

Synchronous  scheduling  is  based  on  cyclic  Interrupts 
generated  by  che  clock.  These  interrupts  initiate  reading 
of  che  sensor  data  and  execution  of  che  controller.  Iinne- 
dletely  after  the  controller  is  executed,  the  new  sec-point 
ia  sent  to  che  manipulator.  When  the  manipulator  executes 
the  set-point,  the  controller  is  waiting  for  a  new  inter¬ 
rupt  signal.  It  is  obvious  that  for  this  type  of  sched¬ 
uling  the  execution  times  of  the  controller  and  the 
manipulator  must  satisfy  the  following  Inequality: 

+  4^''^  S  (22) 

where  T  is  a  fixed  period  of  the  clock.  This  period 
eye 

must  be  chosen  to  satisfy  the  worst  case,  i.e.,  the  maximal 
possible  values  of  At^  and  At^.  As  a  consequence,  there 

will  be  time  intervals  when  both  controller  and  manipulator 
are  idle.  These  intervels  are  indicated  by  the  shadowed 
area  in  Fig.  9.  This  results  in  wasted  time  and  slower 
execution  of  the  composite  action. 

With  asynchronous  scheduling  the  Interrupts  are 
principally  generated  externally  by  che  manipulator  so 
thet  idle  intervala  do  not  exist.  This  scheme  requires 
greater  complexity  in  the  scheduling  mechanism  which 
increases  computer  overhead,  i.e.,  che  execution  time  of 
Che  controller.  Nevertheless,  fsscer  execution  of  compos¬ 
ite  actions  can  be  achieved  by  using  asynchronous 
schedul ing. 

It  is  noted  chat  che  scheduling  problem  is  much  more 
complex  chan  che  brief  remarks  discussed  above.  To  this 
point  che  execution  of  only  one  controller  was  considered. 
As  seen  lecer,  some  complex  eccions  require  simulceneous 
execution  of  more  chan  one  composite  action.  Therefore, 
controller  scheduling  will  involve  parallel  execution  of 
more  than  one  controller. 


6.  IMPLEMENTATION  OF  COMPLEX  ACTIONS 

Execution  of  complex  actions  is  enforced  by  explicit 
commands  issued  by  che  hunen  operecor.  In  ordec  to  study 
Che  ifflplemencecion  of  complex  actions  for  a  given  class  of 
manipulator  casks,  first  che  corresponding  command  struc¬ 
ture  is  considered. 
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rig.  9.  Two  Basic  Approaches  to  Coatrollec  Scheduling 


6.*1  Cotaaand  structure 

Due  to  the  Halted  rcpcrtolr  of  manipulator  tasks 
anticipated  for  the  initial  phase  of  the  CACS  project,  the 
corrccpondlng  coossand  language  ic  quite  claplc.  It  can 
be  defined  by  the  following  cxpresslonc  using  the  Backus* 
Naur  notation: 

{covand^  :  ^onand  code^  ^oaaand  optionc^ 

^paraocter  aesignacntc^ 

<^OB«ii*n<i  cod*>  SEARCH  |  FOLLOW  |  GRASP  I  STOP 

I  HELP  I  HOLD  I  ABORT 

^onaand  opclone^  : ^empcy^  |  ,  ^option  codc^| 

4)pclon  cod«>  :  :•  MANUAL  |  RATE_M0DE  |  DTNAMICJOBJECT 

1  CONSTRAINED_MOTION  (23) 


As  seen,  there  are  seven  basic  comaande,  four  of  which  have 
already  been  diecussed  previoualy.  Cosoand  HELP  enablec 
the  operator  to  interfere  with  controllers  in  order  to 
support  manually  the  automatic  operations  if  it  ie  needed. 
HOLD  cooBDand  Isioediately  freezes  the  motion  of  the  manipu* 
lator  in  emergency  cases.  After  abortion  of  this  coanand 
the  manipulator  will  continue  the  action  from  the  point 
where  it  has  been  interrupted.  ABORT  coasaand  is  ueed  in 
conjunction  with  other  cosnand  codee.  Any  coBDsnd  pre* 
ceded  by  the  parole  ABORT  will  be  vlchdravn  and  disre- 
garded  until  Ir  is  issued  egain.  Cosanand  options  are  also 
discussed  before.  If  an  option  is  not  quoted,  the  corre¬ 
sponding  default  will  be  assumed.  Defaults  arc:  automatic 
operation,  position  control  mode,  static  object,  uncon¬ 
strained  motion,  and  world  coordinate  eyctem. 

The  implementation  of  this  command  language  was 
motivated  by  the  following  requirements: 

e  The  cosnaande  must  be  convenient  for  the  human 
operator,  i.e.,  the  operator  should  be  able  to 
issue  them  easy  atul  fast.  This  is  very  ispor* 
tant  in  the  case  of  an  interactive  MCS. 


I  HAND_COORDINATE_SYSTEM 


(p.ruicccr 
Mclgnacnc* 


<eflipcy> 


^aruwccr  asalgnnenc^ | 


e  Th.  conunds  muec  have  parallel  capablllcy, 
I.e,,  certain  conmanda  will  be  leeued 
s lam  1 1 a neo ua ly . 

e  The  response  of  the  command  interface  must  be 
fast. 


^parameter  assignment^  ^cotaaand  parameter  id^ 

•  parameter  value^ 

^oimaand  paraiseter  id>  :  PI  1  P2  |  P3  I  P4  i  P5  i  P6  |  P7  . 


The  motivee  quoted  above  have  led  to  the  development 
of  a  Universal  Control  Panel  which  is  en  electronic  device 
that  incorporatce  discrete  and  continuous  command  signals 
in  one  device.  The  command  codee  and  option  codes  are 
implMiented  by  a  set  of  "on-off*  switches.  A  switch  has 
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been  sllocaced  co  each  correosnd  code.  The  control  parameter 
ssslgnnencs  are  Impleaenced  by  two  3-diaensionsi  joysticks 
and  s  potentiometer.  These  analog  devices  can  slauitane- 
ously  generate  seven  analog  signals. 

The  human  operator  command  C  which  appears  in  equa¬ 
tions  (16)  snd  (17)  can  now  be  expressed  by  three  vectors: 

C  ■  (£,  £,  £)  (  where  £  ■  (c^ ,  c,, . .  .  .c^)  and  £  • 

(Op  o^ . o^)  are  boolean  vectors  representing  command 

and  option  codas,  respectively,  and  £  •  (p^,  P2,...>p^)  Is 

a  vector  of  rasl  variables  assigned  co  the  parameters.  The 
ordering  of  these  vectors  Is  defined  by  the  expressions 
(23).  For  example,  bit  c^  of  c  corresponds  co  coomiand 
"GRASP",  bit  o^  of  £  corresponds  co  the  option  code 

"MANX'AL",  and  Che  component  of  £  contains  Che  numeric 

value  assigned  to  parameter  P5,  which  Is  relsted  to  the 
output  of  Che  pitch  channel  of  the  right  joystick.  Abor¬ 
tion  of  a  command  Is  done  by  setting  the  corresponding 
switch  In  "off"  position. 


6.2  Event  variables 

To  execute  a  complex  action  some  conditions  oust  be 
satisfied.  First,  the  appropriate  command  oust  be  Issued. 
Second,  the  manipulator  must  be  In  s  proper  state  relstlve 
to  Che  object  so  chat  the  MCS  can  successfully  proceed 
with  the  connanded  action.  For  example,  to  start  the 
FOIXOU  opereclon,  the  EE  oust  be  In  the  proximity  of  the 
object;  or  to  start  the  GRASP  operation  the  MCS  must  cosi- 
plctc  speed  Identification  of  the  object  and  centering  of 
Che  EE.  Therefore,  the  control  elgorlchaw  must  In  every 
moment  of  the  tsak  execution  be  ewers  of  Che  current  stece 
of  the  nanlpuletor  releclve  co  the  object  end  lea  environ¬ 
ment.  For  this  purpose  special  logic  variables,  called 
event  varieblee,  heve  been  Introduced  eleevhere 
(Refs.  20-21).  For  the  CACS  project,  the  event  variables 
are  organised  in  en  event  scetus  scheme  shown  partially 
In  Teble  2. 

The  event  variables  can  be  represented  in  the  vector 
fora  £  ■  (Sp  e2,.*.,e^).  The  components  of  this  vector 

are  boolean  variables  thee  indicate  occurence  of  the  corre¬ 
sponding  event  (when  true),  or  ebaence  of  the  event  (when 
false). 

The  event  scecua  vector  e,  together  with  the  coonand 
code  vector  £  and  the  option  code  vector  o,  provide  suffi¬ 
cient  Information  for  making  the  decision  when  to  start  or 
CO  cermlnece  e  complex  or  s  composite  scclon.  Also,  the 
execution  of  e  composite  action  will  change  the  event 
status  vector  e.  Thus,  the  vectors  £,  o,  end  £  play  en 
essential  role  in  the  coordination  of  both  complex  actions 
end  ths  entire  manipulator  control  task. 


6.3  Action  precedence  graph 

Execution  of  s  complex  action  Is  determined  by  prece¬ 
dence  rules  that  define  the  ordsr  of  execution  of  the 
corresponding  composite  actions.  The  rules  also  specify 
the  conditions  that  must  be  satisfied  co  ecerc  or  co  cer- 
mlnete  the  execution  of  the  actions.  These  rulss  can  be 
expressed  graphically  by  dlegresM  chat  will  be  celled 
Action  Precedence  Graphs  (APG). 

An  A?G  example  Is  given  in  Fig.  10.  The  figure  shows 
four  composite  actions  1,  j,  k,  end  t,  represented  by 
circles.  The  arrows  between  circles  indicate  the  transi¬ 
tions  from  one  action  c.o  another,  end  r,_  ere 

ij’  ji'  ij 

vsrlablai  corraipondlng  to  tha  transition  conditions. 

These  varlablas  ara  boolaan  functions  of  coossand  code, 
option  coda,  and  avant  status  vactors: 

*)  •  “•'>  ’  i.J.'a.t  • 
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TABLE 

CACS  EVENT  STATUS  SCKLIE 

Event 
Variab le 

Description 

e,,e. 

Front- left/right  proximity  sensor 
proximity  distance 

indlCaates 

e,/a. 

3 

Front- left/right  proximity  sensor 
following  distance 

indicates 

Front- let  t/ right  proximity  sensor 
collision  distance 

indicates 

*7/*3 

Lower- left/right  proximity  sensor 
proximity  distance 

indicates 

*9^*10 

Lower- left/right  proximity  sensor 
tracking  distance 

indicates 

41 

Lower- laft/rlght  proximity  sensor 
collision  distance 

indicates 

*13 

EE  Is  roll  aligned  to  the  tracking  plane 

*14 

EE.le  pitch  aligned  to  the  tracking  plane 

*15 

££  Is  yew  aligned  to  the  object 

*16 

Spaad  of  tha  object  Is  identified 

*17 

EE  Is  centered  to  tha  object 

*18 

Object  Is  graaped 

*19 

Object  Is  stopped 

Fig.  10.  Example  of  An  Actions  Precedence  Graph 


The  order  of  indices  of  the  condition  ve^’lebles  indiietes 
Che  direction  of  transition  when  the  conoltlon  beconee 
true. 

Two  kinds  of  transitions  are  shown  in  Fig.  10: 
unblocking  end  blocking  transitions.  Unblocking  trensltlou 
occurs  when  en  action,  say  j ,  realizes  the  truth  of  the 
condition  invokes  the  execution  of  the  succeeding 

action  If  and  continues  Its  own  execution.  After  that  the 
two  actions  are  In  execution  slmultai.eously.  Blocking 
transition  occurs  when  en  action,  say  j  again,  reallzse  the 
truth  of  condition  Invokes  execution  of  the  succeeding 

action  k,  end  lonedletcly  termlnetes  its  own  execution. 

This  type  of  transition  is  Indicated  by  a  point  at  the 
beginning  of  the  arrow.  An  action  can  also  be  terminated 
by  Itself,  without  transition  to  another  action.  This  ie 
indicated  by  en  errowless  point  on  the  circle  and  marked 
by  the  condition  variable  As  shown  in  Fig.  10,  tha 

transitions  esn  be  In  e  reverse  order.  The  condition 

variables  for  reversed  transitions  ere  r^.,  r,  .  and  t..  In 

J1  kj  tj 

rig.  10.  A  raven*  transition  occurs  whan  an  action,  say 
j,  currantly  being  In  execution,  loses  the  condition 

for  Its  active  stet*  and  aust  be  tsraineted,  viiila  its 


(24) 


*  S*  • 


predecessor  1  ausc  be  scclveccd  again.  In  synnccric  cases 
we  have  general  case. 

The  conscrucclon  of  an  APC,  Including  chc  boolean 
expressions  (24),  muse  be  done  with  special  care  co  pre¬ 
sent  deadlocks  or  inscabilicy,  i.e.,  co  prevent  endless 
reversed  transitions  between  evo  actions. 

The  APCs  for  four  c<XBplex  accione  of  the  CACS  arc 
shown  in  Fig.  11.  As  an  exaaple«  chc  logic  expressions 
for  the  transition  conditions  of  the  first  conplex  action 
arc  given  below: 

"12  *  ^  <>4  ''  (*7  ''  V 


-  •  *  A  • 

*3-  •23  *1^ 


•  ■  •  A  (a  ^  a  ) 

“41  34  ^*9  *10^ 


"14  *  "34  ''  ^*9  *10^ 


where  e^,  through  e^^^  are  event  variables  defined  in 
Table  2,  c^  corresponds  to  command  code  for  searching*  o^ 
and  o^  indicate  manual  constrained  options. 

Dotted  arrows  in  Fig.  11  indicate  ths  transitions 
between  complex  actions  for  a  particular  control  cask: 
manual  searching,  following,  grasping,  and  stopping  an 
object  chat  is  slowly  moving  on  a  fixed  plane.  The  doctsd 
arrows  connect  an  APC  for  the  entire  control  cask. 


^USANUAiN 
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^coNTtoiy 

Fig.  11.  Action  Precedence  Graph  of  chc  CACS 


6.4  Cooperating  procssses 

The  composite  actions  shown  in  Figs.  10  and  11  can 
be  implemented  in  software  as  sequential  programs  to  be 
executed  on  sequential  processors.  The  following  questions 


arise:  how  co  implenenc  an  APC  which  requires  parallel 
execution  of  several  composite  actions;  how  to  coordinate 
action  transitions;  and  how  to  exchange  data  among  the 
actions,  i.e.,  among  the  corresponding  sequential  programs. 
The  answer  co  this  is  the  concept  of  cooperative  processes 
largely  employed  in  the  field  of  operating  systems  and 
real-time  programming.  (See  Refs.  13-15,  23-26.)  The 
process  will  be  considered  here  as  an  accivicv  s:  execution 
of  a  sequential  program  on  a  sequential  processor.  In 
general,  the  process  has  two  states:  an  active  state  when 
it  is  in  execution  by  the  processor,  and  a  blocked  state 
when  it  is  not  in  execution.  Furthermore,  the  process  has 
a  form  of  an  endless  cyclic  program,  where  the  cycling  can 
be  terminated  if  the  process  explicitly  blocks  itself,  or 
if  it  is  blocked  indirectly  by  synchronization  mechanisms. 
The  synchronization  mechanisms  are  part  of  a  special  set 
of  procedures  that  support  the  parallel  execution  of 
processes  in  one  or  more  processors.  More  about  this  in 
the  next  section. 

Each  composite  action  has  an  associated  process,  the 
processes  contain  essentially  two  parts:  a  part  where 
they  compute  and  check  the  transition  conditions  and  per¬ 
form  the  corresponding  transition  operations,  and  another 
part  where  they  execute  the  sequential  program  that  imple¬ 
ments  the  controller  of  the  composite  action. 

For  a  transition  operation  two  primitives  are  used: 
AUAK£(i)  and  BLOCK.  The  first  primitive  ie  a  procedure 
that  activates  process  "i"  quoted  in  the  argument  list, 
and  Che  second  procedure  puts  the  calling  process  into  the 
blocked  state.  This  procedure  also  initializes  the  caller 
controller,  i.e.,  it  assigns  the  initial  values  co  the 
state  vectors  r  •  r  and  1  ■  1  ,  so  chat  the  controller 

—  “O  —  -o 

will  be  ready  to  start  again  if  its  process  is  activated 
again.  To  give  an  idea  how  this  works,  consider  the  APC 
in  Fig.  10.  This  example  is  sufficiently  general  co  cover 
all  complex  actions  of  the  CACS.  The  actions  i.  j,  k  and  I 
have  to  be  considered  now  as  processes.  The  process  "J" 
will  have  the  following  form: 

''Process  j" 


compute  transition  conditions; 


If 

* 

true 

Chen 

bcKln  AWAKE (1): 

BLOCK 

end 

if 

■ 

true 

Chen 

AUAKE(l); 

If 

■'jk  ■ 

true 

Chen 

besin  AWAKE (k): 

BLOCK 

end 

if 

true 

Chen 

BLOCK; 

execute  controller  (one  iteration); 


Processes  1,  k  and  I  have  a  similar  form. 

To  this  point  only  processes  chat  perform  compoeice 
actions  are  considered.  But,  for  full  Implemencacion  of 
an  APC,  it  is  necessary  to  cooBunicace  with  the  external 
world,  i.e.,  with  Che  command  interface  and  the  manipulator. 
Thsse  communications  can  be  performed  by  additional  pro- 
.cesses  that  support  various  I/O  functions.  These  proceeses 
are  executed  independently  and  simultaneously  with  the 
action  proceeses. 

7.  SOFTWARE  ORGANIZATION 

Several  software  components  of  the  MCS  have  been  dis¬ 
cussed  in  Che  previous  sections:  motion  primitives  (sub¬ 
section  4,5),  controllers  (subsection  S.1),  two  synchroni¬ 
zation  primitives,  AWAKE  and  BLOCK  (subsection  6.3),  and 
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cwo  kinds  of  processes,  controller  processes  and  I/O  pro* 
ceeses  (subsection  0.4).  In  this  section  a  new  system 
component  will  be  discussed  that  plays  an  important  role 
in  the  Implementation  of  cooperative  processes.  The  new 
component  is  called  monitor. 

The  concept  of  monitors  was  introduced  by  Brinch 
Hansen  (Refs.  13*14)  and  Hoare  (Kef.  IS)  as  a  systematic 
and  powerful  approach  to  the  design  and  implementation  of 
real  time  systems.  This  concept  was  later  extended  to 
high  level  languages  for  concurrent  programming  (Ref.  25- 
26).  Here  the  monitors  will  only  be  diecuased  very 
briefly.  .More  about  monitors  and  their  application  in  the 
MCS  will  be  given  in  a  companion  paper  (Ref.  27). 

Buffers  that  are  used  to  save  data  for  later  use  by 
the  same  part  of  the  program,  or  for  simultaneous  use  by 
different  parte  of  a  program  have  been  discucced  in  previ¬ 
ous  sections.  Examples  are  rotation  matricee  R^  and 

weS  and  JCS  buffers  (Fig.  4),  and  controller  buffers  for 
handling  state  variables  r^  and  ^  (Fig.  7.).  There  are 
also  other  examples,  as  event  variables  e^,  cotomand  data  C, 
and  eeneor  data  S. 

All  these  data  are  shared  among  different  processes 
which  can  run  simultaneously.  In  such  situation,  the 
mutual  exclusion  of  access  to  shared  data  must  be  ensured. 
In  other  words,  it  must  be  guaranteed  that  t.he  same  shared 
data  buffer  can  only  be  accessed  by  one  process  at  a  time. 
If  two  processes  attempt  to  access  the  same  buffer  at  the 
same  time,  one  must  wait  until  the  other  completes  its 
operation  on  the  buffer  data.  It  le  also  important  to 
control  the  access  rights  to  the  shared  data,  i.e.,  it 
must  be  explicitly  declared  who  has  the  right  to  access 
the  shared  data  and  what  it  can  do  with  these  data.  The 
mutual  exclueion  and  the  access  rights  control  of  shared 
data  buffers  is  provided  by  monitors. 

In  general,  a  monitor  ic  an  abstract  software  object 
that  contains  the  following  entitlee:  data  structure, 
procedures.  Initial  operatione,  and  access  right  definl* 
tion.  The  data  structure  contains  the  shared  data  as  well 
as  some  additional  adminietrative  data  necessary  for  the 
functioning  of  the  monitor.  .Monitor  procedures  explicitly 
define  all  operations  which  the  processes  can  perform  on 
the  shared  data.  These  procedures  are  called  directly 
from  the  proccseee,  or  indirectly  from  ocher  system  compo¬ 
nents,  e.g.,  from  controllers  and  motion  primitives.  The 
initial  operations  define  all  operations  chat  must  be 
performed  during  the  time  of  creation  of  the  monitor. 

For  example,  assignments  of  initial  values  r^  and  1^  to 

the  controller  state  variables  £  and  1  are  such  initial 
operations.  Finally,  accees  rights  define  all  connections 
of  Che  aon.^cor  to  the  reec  of  the  sycccm.  This  is  achieved 
by  giving  an  explicit  liec  of  all  proceseee  or  other 
monitors  which  can  accees  the  monitor. 

Monitors  are  used  to  protect  the  shared  data  and  to 
schedule  the  ocher  reeourcee  of  the  system,  as  for  example 
some  programs,  I/O  devices,  and  even  a  processor  or  pro¬ 
cessors.  Monitors  can  also  be  used  to  schedule  the 
processes  that  are  in  producer-consumer  relations. 

Example  for  this  are  sensor  data  buffers  which  are  accessed 
by  at  least  two  processes:  by  an  input  proceee  which  pro¬ 
vides  the  sensor  data,  end  by  a  controller  process  which 
uses  these  data  to  generate  motion  increments.  In  this 
example,  the  input  process  ie  a  producer,  while  the  con¬ 
troller  process  is  a  consumer.  Synchronization  of  these 
processes  can  be  done  if  the  monitor  data  structure  is 
extended  by  two  single  element  queues,  one  for  the  producer 
and  one  for  the  consumer.  The  scheduling  will  work  ae 
follows.  If  ths  consumer  cries  to  cake  the  data  from  the 
empty  buffer  when  the  producer  has  not  yst  provided  the 
data,  it  will  be  blocked  and  placed  into  the  consumer 
queue.  Also,  if  the  producer  cries  to  put  new  data  into 
the  full  buffer  when  Che  consumer  hae  not  yet  taken  the 
previous  data  from  ic,  ic  will  be  blocked  and  placed  into 
Che  producer  queue.  If  the  consumer  hae  taken  the  data 
from  Che  buffer  leaving  the  monitor,  the  monitor  support 
procedures  will  automatically  check  the  producer  queue. 


and  if  it  is  not  empty,  the  producer  process  waiting  in 
this  queue  will  be  activated.  Also,  when  the  producer 
puts  the  new  data  into  the  buffer,  the  consumer  queue  will 
be  checked,  and  the  possibly  waiting  consumer  process  will 
be  activated.  This  method  of  scheduling  can  Oe  used  for 
asynchronous  scheduling  of  the  controller  discussed  in 
subsection  5.3. 

A  monitor  is  allocated  to  all  shared  aaca  in  the 
MCS.  There  ie  no  other  way  to  access  shared  data  except 
by  monitor  procedures.  Some  monitors  are  equipped  by  the 
consumer-producer  facility,  depending  on  the  role  of  the 
monitor  in  the  system.  The  monitore  are  defined  by  the 
application  programmer  who  has  written  the  MCS  software. 
Functioning  of  the  monitors,  i.e.,  sc.heduling  of  simul¬ 
taneous  monitor  calls,  management  of  producer  and  coneumer 
queues,  corresponding  preemption  and  res'mption  of  the 
proceseee,  muat  aleo  be  provided  by  software.  However, 
this  part  of  the  eoftware  is  external  to  the  monitors  and 
it  is  called  ’’kernel."  The  kernel  is  a  collection  of  pro¬ 
cedures  which  support  all  monitore  declared  by  the  appli¬ 
cation  prograoner.  They  implement  also  the  other  synchro- 
alzatlOD  mechanisms  (for  example  AWAKE  and  BLOCK  are  kernel 
procedures),  and  perform  scheduling  of  all  otner  resourcee, 
including  processor  or  proceeaors  in  case  of  a  multipro* 
ceseor  environment.  Therefore,  the  kernel  deai.s  with  I/O 
drivers,  interrupt  handlers,  and  other  communication  and 
synchronization  facilities  implemented  in  t.he  hardware.  On 
the  other  hand,  the  kernel  is  completely  free  of  the  appli¬ 
cation  software,  and  It  can  be  written  by  a  system  pro- 
grasBier  who  is  familiar  with  a  particular  machine,  and  may 
not  be  familiar  with  the  specifics  of  an  application  soft¬ 
ware,  i.e.  with  the  MCS.  So  is  with  the  application  pro- 
graoBer,  who  does  not  have  to  care  about  details  of  hard¬ 
ware  and  communication  and  synchronization  software  facili- 
tiee.  The  kernel  hides  these  details,  extending  the  par¬ 
ticular  machine  to  a  virtual  machine  with  given  character- 
ietles,  pertinent  to  the  MCS  requirements. 

The  whole  MCS  software  can  be  presented  as  a  three 
level  structure  shown  in  Fig.  12.  The  first  (highest) 
level  is  the  manipulator  task  dependent  software.  It  con¬ 
tains  controller  and  I/O  processes.  This  includes  pro¬ 
cessing  of  event  variables  and  transition  conditions,  and 
preparation  of  arguments  for  controllers.  The  second  level 
of  the  MCS  software  contains  the  general  system  components 
like  motion  primitives,  controllers  and  monitore,  which 
can  be  uaed  for  implementation  of  any  complex  action  or 
manipulator  task.  This  level  supports  the  first  level. 

The  third  (lowest)  level  ie  the  kernel.  The  kernel  is  the 
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•  eVAlUATlON  OF  TRANSmON  CONDITIONS 


MOTION  PRIMITIVES,  CONTROlURS  AND  MONITORS 

•  IMRlEMCNriNG  THE  PRIMITIVE  ACTIONS 

•  IMPLEMENTING  THE  COMPOSrTE  ACTIONS 

•  DATA  COMMUNICATION 

•  PROCESS  SYNCHRONIZATION 


•  SUPPORTING  the  MONITORS 

•  supporting  THE  PROCESSES 

•  SCHEDULING  OF  THE  1/0  DEVICES 

•  SCHEDULING  OF  THE  PROCESSOR(S) 

•  U'O  PROCESSING 


Fig.  12.  Structure  of  the  MCS  Software 
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hardvere  dependent  pert  of  the  MCS  software,  and  supports 
the  first  two  levels.  The  complexity  of  the  kernel  depends 
on  Che  synchronization  method  used  for  controller  synchoni* 
ration,  on  the  scheduling  stretegies  used  for  dispatching 
the  processor,  and  on  a  number  of  processors  and  their 
interconnection  technique. 

In  general,  using  this  approach,  the  changes  in 
hardware  will  not  affect  the  first  two  software  levels. 

For  example,  if  the  system  was  running  on  a  given  computer 
and  later  the  original  computer  configuration  is  extended 
to  two  or  more  processors,  only  the  kernel  must  be 
rewritten.  The  other  parts  of  the  MCS  software  will 
remain  unchanged. 


B.  CONCLUSION 

The  structured  approach  to  the  design  and  iaplemente- 
tion  of  interactive  sensor  referenced  HCS  outlined  in  this 
paper  is  related  to  both  control  algoritluBS  and  their  soft'^ 
ware  implementation.  The  control  algorithms  are  based  on 
the  separation  of  the  algorithms  that  perform  the  manipu¬ 
lator  tasks  from  the  local  servo  control  problem.  Accord¬ 
ing  to  this  approach,  the  interective  sensor  referenced 
manipulator  control  is  being  considered  ae  the  problem  of 
execution  of  an  action  network,  and  any  manipulator  task 
is  treated  as  an  arrangement  of  interconnected  actions. 

The  design  epproach  of  this  paper  hae  the  following 
advantages:  (a)  It  clearly  brings  out  the  problem  of  logic 
decision  nets  ea  the  dominating  elementa  in  this  type  of 
control,  (b)  It  modularizes  software  development,  (c)  It 
provides  a  transparent  tool  for  both  prograaners  and  users, 
(d)  It  may  provide  design  guidelines  for  special  purpose 
computing  machines  to  improve  performance  in  this  type  of 
control,  (e)  The  results  can  be  generalized  to  similar 
control  problems. 

Future  work  will  address  the  following  problems: 

(a)  Extension  of  the  existing  manipuletor  task  repertoir 
and  the  corresponding  command  structure,  (b)  Integration 
of  the  existing  command  interface  with  e  voice  connend 
system  developed  previously,  (c)  Further  Investigation 
of  the  action  precedence  graphs  together  with  their  appli- 
cetion  to  cBore  complex  manipulator  tasks  and  analysis  of 
their  structural  properties  including  the  inherent  deadlock 
problem,  (d)  Development  of  new  system  components  like 
motion  primitives,  controllers  and  monitors. 

The  results  will  be  evaluated  experimentally.  The 
project  is  still  in  progress,  and  systematic  experiments 
ere  planned  to  evaluate,  improve,  end  Justify  the  design 
approech. 
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NOTES  ON  COMPUTER-CONTROLLED  ROBOTS 


J.  Y.  S.  Luh 

School  of  Electrical  Engineering 
Purdue  University 

West  Lafayette,  Indiana  47907,  U.S.A. 


I.  INTRODUCTION 

In  the  past  decade,  the  problem  of  productivity  attracted  an  international  atten¬ 
tion.  One  of  the  possible  solutions  of  increasing  productivity  is  to  modernize  the 
manufacturing  facilities  by  means  of  automation.  Among  others,  the  programmable 
automation  is  most  suitable  to  handle  the  !ow  volume  batch  production  of  discrete 
parts.  The  industrial  robots  which  arc  defined  as  computer  controlled  mechanical 
manipulators  used  in  industrial  applications,  fall  into  this  category.  Typically  they  per¬ 
form  the  tasks  of  arc  welding,  paint  spraying,  foundry  operation,  etc.  One  may  assign  a 
robot  to  perform  a  variety  of  job  assignments  simply  by  changing  the  appropriate  com¬ 
puter  program  and  thereby  increase  the  capability  and  flexibility  of  the  industrial 
robots. 

Typically  an  industrial  robot  has  six  joints,  giving  six  degrees  of  freedom,  w'ith  a 
gripper  which  is  referred  to  as  a  hand  or  an  end  effector.  Figure  1  shows  the  commer¬ 
cially  available  Cincinnati  Milacron  Model  T3,  and  Unimation  PUMA  600.  Figure  2 
shows  a  Stanford  manipulator  (l]  which  is  also  an  industrial  robot  by  definition  existing 
among  research  institutes  in  the  United  States.  Each  joint  of  these  robots  is  driven 
hydraulically,  pneumatically  or  electrically  with  a  feedback  control  loop.  As  an  exam¬ 
ple,  a  block  diagram  for  a  joint  control  of  the  Stanford  manipulator  at  the  Purdue 
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University,  which  has  a  permanent  magnet  motor  drive  [2),  is  shown  in  Figure  3. 

There  are  two  groups  of  input  signals  to  the  closed-loop  control  system.  The  first 
group  consists  of  the  desired  compliant  torque  Tj,  the  anticipated  gravity  torque  T^, 
and  the  desired  angular  displacement  6^.  The  values  of  torques  Tj  and  T^  may  be 
computed  using  the  methods  described  in  references  [3]  by  Paul,  [4)  by  Bejezy,  or  [5)  by 
Luh,  Walker  and  Paul,  while  0^  can  be  obtained  from  the  scheme  in  [6]  by  Luh  and 
Lin.  These  signals  actuate  the  system  so  that  enough  voltage  is  applied  to  the  motor 
to  produce  sufficient  torque  to  handle  the  load.  In  the  existing  control  system,  the.se 
three  input  quantities  are  computed  on  a  PDP  11/45  and  loaded  into  LSI-11  micropro¬ 
cessor  w'hich  is  interfaced  to  the  manipulator.  The  second  group  represents  the  reac¬ 
tion  from  the  physical  burden  to  the  robot,  i.e.,  load  T^,  gravitational  torque  Tg,  and 
frictional  torque  f^  of  the  motor- tachometer  assembly.  The  load  Tj^  is  transferred 
through  the  output  shaft  and  geared  down  to  the  motor  shaft.  The  harmonic  drive  has 
a  gear  ratio  of  n  =  1/100.  Besides  the  tachometer  feedback,  the  armature  controlled 
DC  motor  also  has  a  back  emf  with  a  gain  of  Kj,  volts/rad.  contributing  to  the  velocity 
feedback.  In  addition,  an  optical  encoder  is  connected  to  the  shaft  of  the  motor  to  pro¬ 
vide  a  positional  feedback.  In  Figure  3,  Kj  and  Ky  are  gains  in  the  forward  path;  and 
L  and  R  are  the  inductance  and  resistance  of  the  motor-armature  winding,  respectively. 
A  backlash  nonlinearity  is  added  immediately  after  the  gear  reduction  since,  at  the  out¬ 
put  side  of  the  harmonic  drive,  there  is  an  unavoidable  backlash.  Thus,  an  industrial 
robot  is  a  positioning  device  in  that  each  of  its  joints  has  a  positional  control  system. 
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n.  CARTESIAN  AND  JOINT  COORDINATES 


In  reality  a  robot  task  is  naturally  specified  in  terms  of  its  hand  in  Cartesian  coor¬ 


dinates.  It  consists  of  position,  described  by  a  position  vector  £(t),  and  orientation, 
described  by  a  unit  approach  vector  a(t)  and  a  unit  sliding  vector  s(t),  as  indicated  in 


Figure  4(a).  All  these  vectors  are  defined  with  reference  to  the  base  coordinates.  For 


convenience,  a  unit  normal  vector  is  defined  as  n(t)  =  s(t)  x  a(t)  where  x  denotes  the 
“cross  product.”  The  orientation  may  also  be  defined  in  terms  of  Euler  angles  with 
respect  to  the  base  coordinates.  Initially  at  t  =  to,  let  n(to),  s(to)  and  a(to)  align  with 
Xq,  Xo  io,  respectively,  as  shown  in  Figure  4(b).  Any  orientation  |n(t)  s(t)  a(t)j 
may  be  obtained  by  a  rotation  of  ‘y  radians  about  ^  so  that  s(to)  aligns  with  £(t|);  then 
a  rotation  of  P  radians  about  s(ti)  so  that  a(to)  =  a(ti)  aligns  with  a(t),  and  finally  a 
rotation  of  a  radians  about  a(t)  to  obtain  the  required  n(t)  and  £(t).  This  is  equivalent 
to  rotate  the  |n(to)  s(to)  a(to)j  coordinate,  which  aligns  with  ^  Zoj  originally,  a 
radians  about  then  ^  radians  about  and  finally  7  radians  about  ^  again.  These 


three  consecutive  rotations  may  be  represented  by  rotational  operators 


?  =  B.(Zjo.'y)R(lo.i^)B:(L,.«) 


cos  7  -sin  7  0  cos  0  0  sin  cos  a  -sin  a  0 

=  sin  7  cos  7  0  0  10  sin  a  cos  a  0 

0  0  1  0  0  cos  ^  0  0  1 


Since  (n(to)  ^(t^)  a(tQ)]  aligns  with  ^  originally,  then 


n(t)  =V0 
0 


k(t)  =  ?  1 


U(t)  =  ?  0 


V"  V**  ,*•  s’*  .***•"”  .  •  •  *• 
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Now  the  origin  of  [n(to)  s(to)  a(to)]  is  translated  in  the  based  coordinates  Zq] 

with  an  amount  and  in  the  direction  of  £(t).  Thus  a  3  by  4  matrix,  which  describes  the 
orientation  and  position  of  the  hand  at  time  t,  may  be  written  as 


(n(t)  s(t)  a(t)  £(t)] 


I 


10  0  0 
0  10  0 
0  0  10 


+ 


[0  0  0  £] 


(3) 


To  simplify  the  representation  of  the  hand  and  the  mathematical  transformation  of  its 
orientation  and  position,  the  above  operations  described  by  (3)  may  be  written  as 


H(t)  = 


n(t)  sj(t)  a(t)  £(t) 
0  0  0  1 


I 

0  0  0 


£ 

1 


10  0  0 
0  10  0 
0  0  10 
0  0  0  1 


(4) 


which  represents  the  state  of  hand  at  time  t  in  Cartesian  coordinates.  The  4  by  4 
matrix  on  the  right  side  of  the  first  equation  of  (4)  is  referred  to  as  the  coordinate 
frame  of  the  hand  at  time  t  with  reference  to  the  base  coordinates.  The  first  4  by  4 
matrix  in  the  second  equation  of  (4)  is  the  homogeneous  transformation  in  the  base 
coordinates  which  includes  both  rotation  and  translation  [7].  Since,  in  this  case, 
(11(^0)  s(to)  a(to)|  aligns  with  (2L0  JLo  lol  originally,  the  two  4  by  4  matrices  mentioned 
above  are  identical.  More  discussions  on  the  coordinate  frame  and  the  homogeneous 
transformation  are  given  in  Appendix  A. 

From  (2)  it  is  seen  that  the  state  of  hand  at  time  t  in  Cartesian  coordinates  with 
reference  to  the  base  coordinates  may  also  be  represented  by  a  6-dimensional  vector 

Let)'  0{t)'  I  where  la‘)f  =  [a  /?  Tfl  and  (  /  =  transpose  of  (  ).  The  hand,  however. 


is  driven  by  the  actuators  at  the  joints.  Intuitively,  if  all  the  joint  displacements  are 
given,  the  position  and  orientation  of  the  hand  are  determined.  Let  n  be  the  number  of 
joints.  For  i  =  1,  2,  ...,  n,  let  q;  be  the  displacement  of  the  i-th  joint  with  respect  to  its 
own  reference  point.  Then,  for  any  given  robot  with  known  geometrical  dimensions, 
there  is  a  relation 

[2(0'  ^t)'|  =  f[qi,  qj,  q„]  (5) 

where  £(•)  is  a  6  by  1  vector  valued  function.  This  relation  is  known,  but  almost  always 
nonlinear  which  complicates  the  problem  (8|.  Since  in  reality,  one  specifies  |£(t)’  ^(t)'| 
in  Cartesian  coordinates  and  desires  to  determine  the  corresponding  |q|,  ••*,  q„j  so  that 

one  may  command  the  joint  actuators  to  comply  with  the  specification  in  Cartesian 
coordinates.  The  solution  requires  the  inverse  vector  function  TV*)  of  n-dimension. 
This  solution,  if  it  can  be  found,  may  not  be  unique,  for  the  commercially  available 
robots  in  operation,  n  is  usually  either  5  or  6.  The  geometrical  configuration  of  these 
robots  with  proper  definitions  and  ranges  of  q;  enables  one  to  obtain  a  unique  solution 
of  equation  (5)  (8).  The  kinematic  equations  and  their  solutions  are  discussed  further  in 
Appendix  B. 

Knowing  the  transformation  of  position  in  Cartesian  and  joint  coordinates,  it  is 
ready  to  examine  the  following  simple  task.  As  shown  in  Figure  5,  the  robot  is  required 
to  go  to  the  bolt  magazine  to  fetch  a  bolt,  and  place  it  on  top  of  the  bolt  hole  on  the 
beam.  Intuitively  one  expects  that  the  robot  hand  travels  along  the  path  of  straight- 
line  segments,  as  indicated  in  Figure  5,  so  that  it  will  reach  the  bolt  first  and  then 
arrive  at  the  beam.  The  question  is  how  would  one  control  the  joint  actuators  to 
accomplish  the  goal?  Before  one  arrives  with  an  answer,  one  may  examine  the  follow¬ 
ing  two  possible  specifications: 


(a)  Is  the  work  space  free  from  obstacles? 

(b)  Must  the  hand  follow  the  specified  path? 


The  answers  to  each  of  these  two  questions  could  be  either  yes  or  no.  They  combine  to 
form  four  different  classes  of  problems  as  follows; 


Is  the  work  space  free  f ran  obstacles? 


1 

Yes 

1  No 

1  1 

Class  1. 

1  Class  4. 

Mist  the  hand 

1  1 

Positional 

1  Positional  Control 

fol low  the 

1  1 

Control 

1  Plus  Qi-line 

specified  path? 

1  1 

Problan 

1  Col  1  is  ion  Avoidance 

1  1 

1 

1  Travelling 

( 

1  1 

Class  2. 

1  Class  3. 

1  1 

Path 

1  Off-line 

1  1 

Tracking 

1  Col  1 ision- free 

1  1 

Problan 

1  Path  Planning 

1  1 

1  Plus 

1  1 

1  Qi- 1  ine  Path 

1  1 

1  Tracking 

These  problems  will  be  analyzed  briefly  in  the  following  sections. 


III.  POSITIONAL  CONTROL 

If  there  is  no  path  constraints  and  if  the  work  space  is  free,  then  the  controllers 
only  have  to  make  sure  that  the  hand  passes  through  all  the  specified  corner  points  of 
the  path.  It  involves  the  coordinates  transformation,  which  computes  the  correspond¬ 
ing  joint  coordinates  jqj,  ••*,  q„j  of  the  specified  corner  points  in  Cartesian  coordinates 

by  means  of  £'*(•),  and  then  positionally  control  the  robot  in  joint  coordinates  from 
point  to  point.  A  linear  positional  servo  for  each  joint,  such  as  the  one  shown  in  Figure 
3,  will  serve  the  purpose.  With  this  elementary  control  scheme,  the  hand  stops  at  every 
corner  point  (when  the  position  error  vanishes)  before  moves  towards  the  next  corner 
point  which,  from  the  engineering  point  of  view,  is  not  efficient.  Since  each  joint  moves 
with  its  own  velocity  with  no  coordination  of  motions  of  other  joints,  the  traveled  path 
by  the  hand  in  Cartesian  coordinates  does  not,  in  general,  consists  of  straight  line  seg¬ 
ments  between  corners. 

The  input  to  the  control  system  is  the  desired  Cartesian  corner  points  of  the  path, 
which  may  be  numerically  fed  into  the  system,  or  furnished  through  so-called  "teaching 
by  doing,”  i.e.,  corner  points  are  recorded  while  the  hand  of  the  robot  is  led  through 
these  points  manually  by  an  operator. 

Appendix  C  presents  a  detailed  discussion  on  conventional  controllers  design  for 


industrial  robots. 


IV.  PATH  TRACKING 


If  the  robot  is  required  to  travel  along  a  prescribed  path,  the  controller  must  keep 
up  with  path  tracking.  With  positional  controllers  the  path  tracking  can  be  accom¬ 
plished  by  dividing  the  Cartesian  path  into  a  number  of  segments.  Each  end  point  of 
the  segments  is  transformed  into  joint  coordinates,  and  then  the  positional  control  is 
applied  from  point  to  point  in  joint  coordinates.  A  number  of  facts  related  to  this 
approach  should  be  mentioned.  By  transforming  all  the  end  points  of  segments  of 
Cartesian  path,  one  essentially  constructs  n  corresponding  paths  in  joint  coordinates, 

one  for  each  of  the  n  joints.  If  these  segments,  |d£(t/  d^t)J,  are  very  short,  the 
increments  of  joint  displacement,  dqj,  between  adjacent  points  are  very  small  so  that 
sin  dq;  =«  dq;  and  cos  dq;  1.  Thus  the  transformation  £(•)  defined  by  (5)  becomes  a 
differential  transformation  which  is  usually  linear.  This  transformation  is  the  Jacobian 
matrix  of  the  displacement,  which  contains  trigonometric  functions  of  the  joint  dis¬ 
placement  with  respect  to  the  joint  coordinates  before  the  differential  increment  takes 
place  [9].  Analytically,  the  solution  dq;  in  terms  of  d£  and  df  can  be  obtained  simply 
by  inverting  the  Jacobian  matrix.  Although  it  is  sometimes  possible,  it  is  usually 
difficult  since  the  Jacobian  is  quite  complicated.  Numerical  solution  is  also  possible  but 
usually  requires  long  computing  time.  Moreover,  the  Jacobian  matrix  becomes  singular 
when  the  robot  reaches  a  degenerate  position  at  which  the  solution  dq-,  is  not  unique 
(i.e.,  more  than  one  value  of  dq-,  yields  a  same  dp  and  d0.)  An  alternative  method  pro¬ 
ven  to  work  successfully  is  to  differentiate  the  solution  of  equation  (5)  directly  [9].  This 
is  possible  since  for  a  given  robot  with  fixed  dimensions,  the  transformation  f”*  is 
known.  Using  this  approach,  one  must  set  dq;  to  zero  if  it  is  physically  impossible  due 
to  constraints,  or  if  it  is  undetermined.  It  usually  results  in  a  simpler  expression  [9].  A 
detailed  discussion  on  differential  or  Jacobian  transformation  is  included  in  Appendix 


-  9- 


The  desired  Cartesian  corner  points  of  the  path  may  be  fed  into  the  control  system 
either  numerically,  or  through  the  teaching  by  doing  procedure.  The  intermediate 
points  of  the  small  segments  of  the  path  are  usually  specified,  or  interpolated  by  the 
computer-controller,  or  sampled  and  recorded  through  teaching  by  doing. 

By  controlling  the  robot  from  point  to  point  using  the  positional  controller,  it  has 
a  natural  tendency  to  stop  at  each  point.  This  undesirable  phenomenon  may  be  elim¬ 
inated  by  specifying  a  nonzero  velocity  at  each  Cartesian  point,  which  can  be 
transformed  into  the  corresponding  nonzero  joint  velocities  by  an  inverse  Jacobian 
matrix  of  the  velocity.  The  joint  velocities  are  often  referred  to  as  the  “resolved  rate” 
(10).  For  implementation  a  velocity  control  loop  must  be  added  to  each  controller. 
Alternatively,  one  may  take  advantage  of  the  associated  digital  computer  to  compute 
the  required  joint  torques  which  would  yield  appropriate  velocities  at  fixed  accelera¬ 
tions.  The  computation  requires  the  knowledge  of  the  dynamical  equation  of  the  robot. 
Since  the  robot  is  a  nonlinear  mechanism  with  couplings  among  its  joints,  the  computa¬ 
tion  is  time  consuming. 

A  possible  way  to  avoid  the  storage  of  numerous  pre-computed  points  of  joints 
paths  or  the  computation  of  these  points  on-line  is  to  determine  the  functional 
representation  of  n  joint  paths.  However,  the  transformation  /  in  (5)  is  pointwise. 
Since  no  transformation  of  a  function  is  known,  the  curve  fitting  procedure  may  be 
adopted  as  follows  (3,11).  The  corner  points  of  the  Cartesian  path  are  first  transformed 
into  joint  coordinates.  A  cubic  function  is  assigned  to  each  segment  between  every  two 
adjacent  joint  coordinate  points.  The  adjacent  cubic  functions  are  then  splined 
together  with  the  continuity  conditions  of  desired  velocity  and,  if  required,  desired 
acceleration.  Curve  fitting  is  done  by  using  a  large  number  of  points  on  the  path,  after 
they  are  transformed  into  joint  coordinates,  and  applying  the  least  square  error  fit  to 
obtain  the  coefficients  of  the  spline  function.  Thus  one  needs  to  store  only  the 
coefficients.  The  points  on  the  joint  paths  can  be  obtained  by  a  simple  computation 
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when  needed. 

An  alternative  method  to  the  above  mentioned  differential  straight  line  motion  is 
the  on-line  evaluation  and  decision  making  scheme  developed  by  Paul  [12].  the  scheme 
is  divided  into  three  processes:  (a)  first  the  current  position  and  velocity  of  the  hand  in 
Cartesian  coordinates  are  evaluated  for  the  path  accuracy  to  decide  whether  a  ch.ange 
in  velocity  is  needed;  (b)  points  on  the  path  of  hand  between  which  the  change  of  velo¬ 
city  takes  place,  are  generated  and  transformed  into  the  corresponding  points  in  joint 
coordinates;  and  (c)  finally  a  quadratic  interpolation  between  these  points  in  joint  coor¬ 
dinates  is  determined  for  the  robot  to  follow.  These  processes  are  repeated  frequently 
at  a  sampling  rate  not  less  than  the  structural  resonant  frequency  of  the  robot.  Taylor 
[13]  employs  the  quaternion  representation  [14]  for  the  coordinate  frame  of  the  hand 
instead  of  the  homogeneous  transformation  representation,  so  that  the  rotational  opera¬ 
tions  require  less  computations.  The  translational  operations  however,  do  not  yield  any 
advantage.  In  his  approach,  enough  intermediate  points  in  the  joint  coordinates  are 
added  so  that  the  path  is  determined  by  linear  interpolation  which  is  restricted  by  a 
prescribed,  maximum  allowable  path  deviation.  Thus  the  computation  is  simpler  than 
that  for  the  case  of  quadratic  interpolation. 

V.  COLLISION-FREE  PATH  PLANNING 

If  the  work  space  is  not  free  from  obstacles,  then  it  is  best  to  determine  a 
collision-free  Cartesian  path  and  then  impose  the  path  tracking  requirement.  Conven¬ 
tionally  the  path  is  created  by  the  operator  and  transferred  to  the  computer  storage  for 
the  robot  by  recording  a  sequence  of  path  points  while  the  operator  leads  the  robot 
hand  through  the  path.  When  the  work  space  near  the  goal  point  is  crowded  with  obs¬ 
tacles,  the  teaching  by  doing  method  by  operator  becomes  insufficient,  especially  if 
minimum  execution  time  or  distance  is  imposed. 


The  subject  of  collision-free  path  planning  is  relatively  new.  Within  the  past  four 
or  five  years,  only  a  handful  people  are  actively  working  on  this  topic  (15-20].  Essen¬ 
tially  the  robot  is  treated  as  a  point  by  suitably  expanding  obstacles  to  compensate  for 
robot  body  dimensions.  A  2-dimensional  example  of  robot-environment  of  Lozano- 
Perez  is  shown  in  Figure  6  as  an  illustration.  A  triangular  cross-section  of  robot  is 
required  to  move  around  a  stationary,  rectangular  obstacle  without  any  rotation. 
When  the  triangle  touches  the  rectangle  and  slides  around  it,  the  lower  left  corner  of 
the  triangle  yields  a  hexagon  around  the  rectangle.  Thus,  if  the  robot  is  treated  as  a 
point  located  at  its  lower  left  corner,  the  rectangular  obstacle  must  be  expanded  to  a 
hexagon  to  compensate  the  robot  body.  The  concept  can  be  extended  to  three- 
dimensional  case  with  moving  obstacles.  One  must  be  cautious  during  the  final 
approach  to  a  goal  so  that  the  expansion  does  not  engulf  the  goal  point.  Using  a  set  of 
heuristically  derived  rules,  it  is  possible  to  develop  an  algorithm  which  determines  a 
feasible  and  best  (in  the  sense  of  either  minimum  execution  time  or  minimum  distance 
traversed)  path  [20].  Intuitively  the  algorithm  should  be  implemented  with  an  interac¬ 
tive  computer  graphics  display  of  a  robot  in  various  environments  which  allows  simula¬ 
tion  of  the  robotic  assembly  line.  However,  one  must  be  careful  about  the  hidden  lines 
of  the  2-dimensional  projection  since  they  essentially  indicate  whether  there  is  a  colli¬ 
sion. 

VI.  COLLISION  AVOIDANCE  TRAVELLING 

The  problem  discussed  in  Section  V  involves  the  conditions  that  the  work  space  is 
not  free  from  obstacles  and  that  the  collision-free  paths  must  be  followed.  If  the  second 
condition  is  not  imposed,  then  the  robot  is  free  to  move  anywhere  in  the  space  which 
runs  a  risk  of  collision.  Thus,  when  the  positional  control  is  implemented,  an  on-line 
collision  avoidance  capability  is  needed  to  ensure  the  safety  of  the  operation.  Very  lit¬ 
tle  research  results  are  known  in  this  area.  A  common  practice  is  to  stop  the  motion 
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whencver  an  obstacle  in  the  path  is  detected. 

VII.  COMMAND  LANGUAGE  AND  TASK  DESCRIPTION 

To  delegate  the  task  of  controlling  a  robot  to  a  computer,  it  is  necessary  to  estab¬ 
lish  communications  between  the  computer  and  the  operator  through  a  command 
language.  Practically  for  each  different  product  of  robot,  there  is  a  different  command 
language.  Each  was  developed  for  its  own  goal  tasks,  and  unfortunately  they  are  not 
unified. 

The  first  of  such  language  may  be  traced  back  to  LISP  [21].  At  the  Stanford 
University,  Paul  [22]  originally  developed  WAVE  programming  language  which  is  capa¬ 
ble  of  performing  assembly  by  accepting  force  and  visual  sensing  inputs  and  computing 
joint  torques  using  Lagrangian  formulation  of  dynamic  equation.  It  is  later  elevated  to 
high  level  language  AL  by  Finkel  [23]  in  which  AFFIX  statements  are  used  for  expres¬ 
sions.  The  drawback  is  the  involvement  of  vast  amount  of  computations.  To  simplify 
the  implementation,  POINTY  [24]  was  developed  but  unfortunately  it  is  still  compli¬ 
cated.  Based  on  WAVE,  Shimano  [25]  developed  the  robot  control  language  VAL  for 
Unimation  Inc.  which  simplifies  the  task  by  interpreting  the  program  on  line  by  line 
basis.  Independently  at  IBM  Research  Center,  Lieberman  and  Wesley  developed 
AUTOPASS  [26]  as  a  model-based  program  automation  tool  while  Summers,  Taylor 
and  Meyer  created  AML  [27]  which  is  a  semantically  interactive  language.  There  are 
many  other  programming  languages  in  use  developed  in  U.S.A.  and  abroad.  Each  of 
them  is  a  complete  language  system  which  includes  scanners,  parsers  and  symbol  table. 
In  Italy  Gini  and  Gini  [28]  worked  on  a  goal  orientated  language  while  Bernorio  [29]  et 
al.  developed  a  quasi-natural  language.  A  language  for  part  assembly,  RAPT  [30]  was 
introduced  by  Popplestone  et  al.  of  University  of  Edinburgh,  Scotland.  Falek  and 
Parent  [31]  in  France,  and  Blume  and  Dillmann  [32]  in  Federal  Republic  of  Germany 
.also  developed  robotic  languages  independently.  At  Purdue  University,  Paul  [33]  takes 
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a  different  approach  by  embedding  the  motion  primitives  and  data  structure  into  a  high 
level  computer  language  PASCAL  thereby  eliminating  the  development  of  his  own 
parser,  etc.  He  combines  the  joint  coordinates  control  with  Cartesian  coordinates  task 
description  to  include  compliance  for  part  assembly  using  the  joint  torque  sensing.  The 
resulting  language  is  called  PAIj  (34)  (Purdue  Arm  Language)  which  is  in  the  proce.ss  of 
further  development,  and  which  uses  the  homogeneous  transformation  to  describe  the 
variations  of  hand  position  and  orientation  of  the  robot.  In  the  following  the  homo¬ 
geneous  transformation  approach  of  PAL  is  used  to  illustrate  how  the  task  of  the  robot 
is  described  and  its  motion  is  commanded. 

PAL  is  developed  to  suit  the  position-dependent  structure  of  robots’  tasks.  By 
assigning  a  Cartesian  coordinate  system  to  each  object  in  the  work  space,  each  object 
can  be  represented  by  a  4  by  4  matrix,  similar  to  H(t)  defined  by  (4),  with  respect  to  a 
chosen  reference  coordinates.  Any  specific  geometry  on  the  object  can  then  be 
described  by  a  4  by  4  matrix  in  terms  of  its  position  and  orientation  with  respect  to  its 
own  objects’  coordinates.  Thus,  when  the  object  moves,  there  is  no  need  to  up-date  the 
4  by  4  matrix  of  the  geometry  on  that  object.  For  convenience,  all  the  4  by  4  matrices 
are  called  the  Cartesian  coordinate  frames.  If,  for  some  reason,  the  reference  coordi¬ 
nates  of  a  frame  need  to  be  changed,  i.e.  changing  its  position  and/or  orientation,  it 
can  be  done  by  a  simple  homogeneous  transformation  [7],  i.e.,  premultiply  the  subject 
frame  by  a  4  by  4  matrix  which  describes  the  old  reference  coordinates  with  respect  to 
the  new  coordinates. 

Using  an  example  in  Reference  [35]  as  an  illustration,  refer  to  Figure  7.  The  task 
.of  the  robot  hand  is  to  lift  the  bracket  by  its  handle  and  set  it  on  top  of  the  beam  to 
align  their  bores,  and  then  fetch  a  bolt  and  place  it  in  the  bore  of  bracket.  To  set  up 
the  task,  select  a  convenient  WORLD  coordinates  as  a  Cartesian  base  coordinates.  Its 
frame  is  simply  a  4  by  4  identity  matrix.  Then  define  Z  with  respect  to  WORLD  to 
represent  the  robot,  define  T6  with  respect  to  Z  to  represent  the  robot  wrist,  and  define 
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E  with  respect  to  T6  to  represent  the  robot  end-effector  (or  the  hand  in  this  example.) 

The  objects  must  be  specified  in  the  same  base  coordinates.  Thus  define  BEAM, 
BOLT  and  BRACKET  with  respect  to  WORLD.  The  geometries  on  the  objects  are 
defined  as  follows;  BEAM-BORE  with  respect  to  BEAM;  BRACKET-HANDLE, 
BRACKET-BORE,  and  BOLT-HOLE  i(i  =  1,  4)  with  respect  to  BRACKET;  and 

BOLT-GRASP  with  respect  to  BOLT.  The  coordinate  structure  is  as  follows: 


HE^M 


WHO 

I 

ffi/^CKET 


BXT 


Obj  ects  ^ 


ie: 


EE^M-DCRE  BCLT-HXE  i  eWKET-BT^E  IR/^KEl'-HV^JE  BCLT-O^ASP 

I  I 


I 

E 


I 

E 


Robot  < 


T6  T6 

Z 

I 

WHO 


To  make  it  possible  for  the  robot  to  perform  the  task,  the  following  position  equations 
must  be  satisfied: 


Z  •  T6  *  E  =  BRACKET  •  BRACKET-HANDLE 


(6) 
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and  Z  •  T6  *  E  =  BOLT  •  BOLT-GRASP 


(7) 


These  two  equations  allow  the  end-effector  making  contact  with  the  BRAOKF/T- 
HANDLE  and  with  BOLT-GRASP.  From  (6)  and  (7)  it  is  seen  that  BRACKET  can  bo 
expressed  as  Z  *  T6  *  E  *  (BRACKET-1  lANDLIC)"'  and  BOLT  as 
Z  *  T6  *  E  *  (BOLT-GRASP)"*.  Thus  for  BRACKET-BORE  contacting  BEAM- 
BORE,  one  requires 

Z  •  T6  *  E  *  (BRACKET-HANDLE)-'  *  BRACKET-BORE  =  BEAM  *  BEAM-BORE  (8) 


Likewise,  from  (7)  and  (8),  one  obtains  BOLT  =  Z  *  T6  *  E  *  (BOLT-GRASP)*^  and 
BRACKET  =  BEAM  *  BEAM-BORE  *  (BRACKET-BORE)* ^  The  final  requirement 
is  to  satisfy 


Z  *  T6  *  E  *  (BOLT-GRASP)-'  =  BEAM  •  BEAM-BORE 

•  (BRACKET-BORE)-'  •  BOLT-HOLE  i  (9) 

Equations  (6)  through  (9)  form  the  basis  of  the  positional  requirement  for  the  task.  For 
the  convenience  of  programming,  define  two  macro  symbols 


ARM  ::=  NAME  *  Z  *  T6 


(10) 


and  TOL  :;=  E  (11) 


where 


(BRACKET)'^  for  cq.  (6) 

(BOLT)  for  cq.  (7) 

[BEAM  *  BEAMBORE  *  (BRACKET-BORE)'']-'  for  cq.  (8) 

(BEAM  *  BEAM-BORE  *  (BRACKET-BORE)''  *  BOLT-HOLE  i]''  for  eq.  (9) 


Then  the  requirement  becomes 


BRACKET-HANDLE,  for  cq.  (6) 
BOLT-GRASP,  for  cq.  (7) 
BRACKET-HANDLE,  for  cq.  (8) 
BOLT-GRASP,  for  cq.  (9) 


NAME  = 


To  manipulate  the  robot,  introduce  a  MOV  statement  such  that  MOV<expression> 
•  yields  a  motion  of  positioning  the  robot  such  that 


ARM  *  TOL  =  <exprcssion> 


Thus 


BRACKET-HANDLE,  for  eq.  (6) 

=  =  >  robot  contacts  BRACKET-HANDLE 
BOLT-GRASP,  for  cq.  (7) 

=  =  >  robot  contacts  BOLT-GRASP 
expression  =  BRACKET-HANDLE,  for  eq.  (8) 

=  =  >  BRACKET-BORE  contacts  BEAM-BORE 
BOLT-GRASP,  for  eq.  (9) 

=  =  >  BOLT  contacts  BOLT-HOLE  i 


For  further  discussions,  readers  are  referred  to  Chapters  5  and  10  of  Reference  [33]. 
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APPENDIX  A 

COORDINATE  FRAME  AND  HOMOGENEOUS  TRANSFORMATION 

In  Section  11,  equation  (4),  it  is  shown  that  the  hand  of  the  robot  can  be  described 
by  a  4  by  4  hand  matrix: 

n  £  a  £ 

0  0  0  1 

where  n,  s  and  a  are  orthonormal  unit  vectors  describing  the  orientation  of  the  hand, 
and  £  is  the  position  vector.  All  the  vectors  are  defined  with  respect  to  the  base  coordi¬ 
nates,  hence  the  hand  matrix  represents  the  orientation  and  position  of  the  hand  with 
reference  to  the  base  coordinates.  In  H,  the  3  by  3  orientation  matrix  [n,^^  can  be 
viewed  as  a  coordinate  system  (Xn,In,z„]  with  respect  to  base  coordinates  for  the  joint  n 
of  the  robot  having  n  joints  (i.e.,  the  farthermost  joint  away  from  the  base).  This  coor¬ 
dinate  system  is  fixed  on  joint  n  so  that  it  moves  with  joint  n.  Consequently  rotating 
and  then  translating  joint  n,  and  hence  the  hand,  with  respect  to  base  coordinates  is 
the  same  as  rotating  and  then  translating  respect  to  the  base  coordi¬ 

nates.  Thus  the  resulting  orientation  and  position  of  the  hand  after  some  rotation  and 
translation  can  be  computed  by  premultiplying  H  by  an  appropriate  homogeneous 
transformation  corresponding  to  the  specified  rotation  and  translation. 

From  equation  (4),  the  first  4  by  4  matrix  in  the  second  equation  is  a  homogeneous 
transformation.  It  has  the  form  of 

ft  £ 

0  0  0  1 

in  which  V  is  a  3  by  3  rotation  matrix  and  £  is  a  3  by  1  translation  vector.  Because  V 
is  a  rotation  matrix,  its  three  columns  are  orthonormal  vectors  with  a  unity  norm. 
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A. 


Thus  the  homogeneous  transformations  for  pure  rotation  and  pure  translation  are, 
respectively, 


V  0 

I  0  0 

0 

0  1  0  £ 

0 

and 

0  0  1 

0  0  0  1 

0  0  0  1 

so  that 


1  0  0  £ 

0 

0  1  0 

V  0 

V  £ 

0  0  1 

0 

0  0  0  1 

0  10  1 

0  0  0  1 

Note  that 


V  0 

1  0  0  p 

0 

0  1  0  " 

1  £ 

0 

0  0  1 

0  0  0  1 

0  0  01 

pool 

Supposing  the  hand  aligns  with  the  base  coordinates  initially  so  that 


2Ln'’  In”  L?  e'’ 
0  0  0  1 


10  0  0 
0  10  0 
0  0  10 
0  0  0  1 


is  the  initial  coordinate  frame  of  the  hand  with  respect  to  base  coordinates.  Now  rotate 
and  then  translate  the  hand  by  ?  and  £  respectively  so  that  the  final  coordinate  frame 
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*  1 


of  the  band  is 


1  0  0  £ 

V  0 

2Ln  In  in  £ 

0  10 

0 

in“  In“  L?  £“ 

0  0  0  1 

0  0  1 

0 

0  0  0  0 

0  0  0  1 

0  0  0  1 

1  B 


[0  0  0  1 

Thus 


n  s  a  £ 

S.  E 

0  0  0  1 

000  1 

which  is  identical  to  equation  (4). 

Let  Tfq)  be  a  translational  operator  which  is  a  4  by  4  matrix.  If  the  hand  is 
further  rotated  0^^  radians  about  x^-axis,  then  9y  radians  about  y^-axis,  then  radians 
about  z„-axis,  and  finally  it  is  translated  q  with  respect  to  the  base  coordinates;  the 
final  orientation  and  position  of  the  hand  with  reference  to  the  base  coordinates  is  thus 
given  by 


n  5  a  £ 
0  0  0  1 


liBtl 


=  1(9) 


R(i0.«)  0 

Rfyo.^,)  0 

R(io.^)  0 

E 

0 

0 

0 

R 

0 

0 

0 

0  0  0  1 

0  0  0  1 

0  0  0  1 

0  0  0  1 

i-- 


i 


u 

S'.' 

“•V 

N 
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1 

0 

0 

cos^, 

-sintf, 

0 

0 

COSBy  0 

sin^,  0 

?! 

0 

1 

0 

9 

9in&, 

costf, 

0 

0 

0  1 

0  0 

i-M  = 

0 

0 

1 

0 

0 

1 

0 

-sintfj  0 

cosBj  0 

0 

0 

0 

1 

0 

0 

0 

1 

0  0 

0  1 

10  0  0 
0  costf,  ~9intf,  0 
0  sin0,  cos®,  0 


^  2 

[0  0  0  1 


The  idea  of  coordinate  frame  may  be  applied  to  an  object  to  describe  the  change 
of  its  orientation  and  position.  To  illustrate  the  point  consider  two  identical  wedges 
which  are  placed  in  a  scene  coordinate  system  [Xg,Xg,^l  as  shown  in  Figure  8.  It  is 
required  to  relocate  them  as  shown  in  Figure  9.  First,  the  two  wedges  are  labelled  arbi¬ 
trary  Wl  and  W2  in  both  figures.  Obviously  Wl  may  reach  its  required  location  if  it  is 
rotated  90°  about  and  then  90*  about  z^.  Mathematically,  coordinate  systems 

(xwi’IvvDiwil  l2LW2'IW2'iW2l  reference  to  [2L,)2;«)isl  assigned  to  Wl  and  W2, 
respectively,  as  shown  in  Figure  10.  Then 


JLwi  Iwi  iwi  0 

iW2  I,W2  iW2  0 

0 

5 

0 

and 

0 

0  0  0  1 

0  0  0  1 

are  the  two  coordinate  frames  for  Wl  and  W2,  respectively,  with  respect  to  [Xa,ya,Zg). 
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Thus  the  final  location  of  Wl  may  be  achieved  by  the  following  operations 


R(^,90°)  0 

R(x^,90°)  0 

2Lwi  Iwi  iwi  £wi 

0 

0 

25.W1  Iwi  iwi  Ewi 

0  0  0  1 

final 

0 

0 

0  0  0  1 

0  0  0  1 

0  0  0  1 

Since  sin  00  ”  =  1  and  cos  90°  =0,  and  in  addition,  £yv,  =  (0,0,0)  and  [xwi.Jlwi'iwil 
aligns  with  initially;  then  the  above  expression  reduces  to 


1 

0-100 

10  0  0 

10  0  0 

iwi  Iwi  iwi  Ewi 

10  0  0 

0  0-10 

0  10  0 

0  0  0  1  lual  ^ 

0  0  10 

0  10  0 

0  0  10 

0  0  0  1 

0  0  0  1 

0  0  0  1 

0  0  10 
10  0  0 
0  10  0 
0  0  0  1 


Thus 


ii 


0 

0 

1 

Xwij  final  - 

1 

-  £.Wlj  final  = 

0 

-  Iwij  final  - 

0 

0 

1 

0 

£wi|  final  -  0 


which  implies  Xwi  finally  aligns  with  y^wj  with  z^vvi  with  and  there  is  no  trans¬ 
lation. 


4 


i 


•***. 


L\* 

3 


u 
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Assume  that  the  wedges  are  rigid  bodies.  Then  the  same  rotational  operators 
apply  to  every  point  on  the  wedge.  As  an  example,  consider  the  corner  point  on  Wl 
represented  by  the  vector  [-1,4,0]  (see  Figure  8).  To  apply  the  homogeneous  transfor¬ 
mation,  a  “1”  is  added  to  the  vector  as  the  fourth  element  so  that  the  final  location  of 
the  corner  point  may  be  computed  as: 


H..- 


0 

-1 

0 

0 

1 

0 

0 

0 

-1 

0 

1 

0 

0 

0 

0 

0 

-1 

0 

4 

-1 

0 

0 

1 

0 

0 

1 

0 

0 

0 

4 

0 

0 

0 

1 

0 

0 

0 

1 

1 

1 

which  implies 


0 


Intuitively,  for  W2  to  reach  its  final  location,  it  may  be  rotated  -90°  about 
then  90  °  about  and  finally  translated  9  units  in  the  positive  direction.  The  final 
location  of  W2  may  thus  be  computed  as 


r 

1  00 

0 

R(i„90 ' )  0 

• 

iW2  31W2  i,W2  2W2 

0  1  0 

0 

0 

0  0  0  1 

Inml 

00  1 

9 

0 

*  -j 

000 

1 

0  0  0  1 

R(x,-90 " )  0 
0 
0 

0  0  0  1 


XW2  3^W2  iW2  2W2 
0  0  0  1 


10000  -1  001  0  00  1000 
01001  0  0000  10  0105 
00190  0  100  -1  00  0010 
000100010001  0001 


0  0-10 
10  0  0 
0-104 
0  0  0  0 


so  that 


0 

1 

.  J!.W2|  tB»i  - 

0 

0 

>  iLw2|aBki  ~ 

-1 

0 

>  EwzI  anti  = 

0 

0 

p 

-1 

0 

4 

Initially,  £yv2  indicates  that  the  origin  of  the  coordinate  frame  of  W2  is  5  units  away 
from  the  origin  of  coordinates  fx„y.,z^|  along  the  positive  ^  direction.  Finally  the  ori¬ 
gin  of  the  coordinate  frame  is  4  units  away  from  the  origin  of  along  the  posi¬ 
tive  7^  direction.  The  final  orientation  is  that  aligns  with  negative  z^, 

and  zy/2  with  negative 

In  the  above  discussion,  it  is  seen  that  an  operation  often  involves  a  sequence  of 
rotations.  Mathematically  this  is  represented  by  the  product  of  a  corresponding 
sequence  of  matrices  in  a  proper  order.  Since  the  product  of  the  rotation  of  matrices  is 
also  a  rotation  matrix,  then  the  sequence  of  rotations  in  base  coordinates  is  equivalent 
to  a  single  rotation  of  an  appropriate  angle  9  about  an  appropriate  axis  of  rotation  t_ 
where  £  is  a  unit  vector  in  the  same  base  coordinates.  Now  the  problem  is  to  determine 
9  and  r. 
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To  do  this,  let 


u  V  r  o 


^“  0  0  0  1  -  5  = 


be  a  coordinate  frame  with  reference  to  base  coordinates.  A  is  obtained  by  a  rotation 
of  the  base  coordinates  such  that  its  z-axis  will  align  with  r^  the  z-axis  of  A.  The  direc¬ 
tions  of  u  and  v  are  arbitrary  provided  u,  v  and  r  are  orthonormal  such  that  jr  =  u  x  v . 
Thus  A“*  =A’,  i.e.  A  inverse  equals  the  transpose  of  A.  By  definition,  then,  A 
transforms  the  z-axis  of  base  coordinates  into  r,  or 


=  A 

1  1 


=^i 


Conversely,  A"*  transforms  r  into  the  z-axis  of  base  coordinates,  or 


=  A-‘  =  A' 

1  1  1 


Hence  a  rotation  of  an  angle  6  about  £  in  coordinate  frame  A  is  the  same  as  a  process  of 
the  following  three  steps: 

(a)  transforming  the  A  frame  back  to  the  base  coordinates  (this  involves  an  operation 
of  A“*  or  a’  ); 

(b)  rotating  about  z-axis  of  base  coordinates  an  angle  0,  i.e.,  R(z,^);  and 

R(z,0)  0 

0  , 

(c)  transforming  the  resulting  frame  q  A  back  to  the  A  frame  (this  involves 

0  0  0  1 


an  operation  A). 


Mathematically,  the  equivalence  relation  can  be  expressed  as 


R(r,(?)  0 
0 
0 

0  0  0  1 


=  A 


R(z,tf)  0 
0 
0 

10  0  0  1 


Ux 

Vx 

fx 

0 

cos^ 

-sin^ 

0 

0 

Ux 

“t 

U| 

0 

''t 

h 

0 

sind 

coad 

0 

0 

V, 

V, 

0 

U. 

V, 

r. 

0 

0 

0 

1 

0 

Tx 

tt 

0 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

1 

The  right  side  of  the  above  equation  involves  multiplications  of  three  4  by  4  matrices. 
Through  a  tedious  algebraic  manipulation,  one  obtains,  from  above,  a  sum  of  a  sym- 
metrix  and  a  okew  symmetrix  matrix  for  R(r,^): 


RM)  = 


^X  fj^x  flfx 

cos^ 

-r^sin^ 

TjSin^ 

^x^j  T,Tj 

(1  -  cos^)  + 

r.sin^ 

cos^ 

-r,sinP 

r^r,  r,* 

-r^sin^ 

r,sin^ 

coad 

. 

Now  let  P-[ns^  be  a  product-rotation  matrix  which  is  the  product  of  a 


-  30- 


sequence  of  rotation  matrices  in  an  appropriate  order.  By  setting 
P  =  R(r,0) 

one  specifies  0  and  £  as  the  equivalent  angle  and  axis  of  rotation,  respectively, 
corresponding  to  the  sequence  of  rotations  mentioned  above.  To  solve  for  0,  add  the 
diagonal  terms  of  the  matrix  of  each  side  together  to  yield 

nx+  Sy+  a,  =  (rx+  r“+  rx^)(l-cos0)+  3  cosB 

Since  £  is  a  unit  vector,  the  above  reduces  to 

COS0  =  (nx+ Sy+ a,-l)/2 

Although  this  is  a  simple  formula  for  computing  the  value  of  6,  an  excessive  error  may 
result  if  the  abfiolute  value  of  the  right  .side  is  close  to  unity.  In  addition,  there  is  an 
ambiguity  .so  far  as  the  quadrant  is  concerned.  An  alternative  choice  is  to  take  the 
differences  of  off-diagonal  terms  to  yield  three  equations: 

ny-Sx  =  (rxry-ryrx)(l-cos0)+ r,sin^(-r,sinf?) 

=  2rxSin6> 
n,-ax  =  -2rySin0 
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s,-ay  =  2rj5sin0 

Since  £  is  a  unit  vector,  the  components  of  in  the  above  three  equations  may  be  elim¬ 
inated  by  first  squaring  each  equation  individually,  and  then  adding  them  together. 
This  yields 

\/(ny-sJ''^+  (n,-aJ‘-^+  (s,-ay)72  =  s\n0 

where  the  positive  sign  of  the  square-root  is  chosen  for  0  <  <?  <  180° .  Likewise,  the 
ambiguity  in  quadrant  and  excessive  error  when  the  left  side  having  a  very  small  value 
still  exist.  A  third  possibility  is  to  compute  the  ratio  of  these  two  formulas: 

0  -  tan"‘  [\/(ny-sj2-f  (n (s,-ay)‘‘^]/(n,+  Sy+  a -1) 

The  quadrant  is  thus  determined  by  the  sign  of  the  denominator. 

Once  0  is  determined,  the  axis  of  rotation  £  may  be  determined  from  the  three 
equations  already  derived  by  taking  the  differences  of  off-diagonal  terms.  This  yields 

Tx  =  (s,-ay)/(2sin0) 

ry  =  (a,,-nj/(2sin0) 

r,  =  (ny-sj/(2sin0) 

One  must  make  sure  that  r^  +  r^  +  r^  =  1.  This  condition,  however,  may  be  difficult  to 
satisfy  when  sin  0  is  small.  In  this  case,  one  may  equate  the  diagonal  terms  of 
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P  —  R(r,^)  to  yield 

1,  =  r^(l-cos0)+  cos^ 
iy  =  ry(l-cos^)+  COS0 
i,  =  r,^(l-cos0)+  cos^ 


Whenever  the  hand  of  the  robot  and  the  object  have  their  own  coordinate  frames, 
it  is  a  simple  matter  to  describe  mathematically  the  contact  between  them.  Let 


n  s  a  £ 

H  =  ”  ”  and  W  = 

0  0  0  1 


iW2  IW2  iW2  2W2 
0  0  0  1 


be  the  coordinate  frames  for,  respectively,  the  hand  and  the  wedge  W2  defined  previ¬ 
ously.  Intuitively  one  would  set  H  =  W  to  describe  their  rendezvous.  This  intuition, 
however,  is  false  since  H  and  W  are  defined  with  reference  to  two  different  coordinate 
systems.  Thus  to  achieve  the  goal  of  describing  the  rendezvous,  one  must  have  the 
coordinate  frames  for  the  hand  and  the  wedge  referring  to  a  same  coordinate  systems. 

io  io  Jp  £o 

Call  this  the  world  coordinates,  and  let  the  base  coordinate  frame  and  the 

[0  0  0  1 

scene  coordinate  frame  be  defined  in  the  world  coordinates  (see  Figure  11). 

0  0  0  ij 
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Then 


X  0  io  *0  £o 

n  s  a  £ 

0  0  0  1 

0  0  0  1 

specifies  the  hand  in  world  coordinates  while 


L,  L,  Za  Ea 

Z.W2  IW2  iW2  EW2 

0  0  0  1 

0  0  0  1 

specifies  the  wedge  in  the  same  world  coordinates.  By  equating  these  two  expressions, 
one  specifies  the  rendezvous.  Usually  the  base  coordinate,  the  scene  coordinate  and  the 
wedge  coordinate  frames  are  either  given  or  measured.  Thus  one  may  compute  the 
required  coordinate  frame  or  location  of  the  hand  for  rendezvous  as 


n  s  a  £ 
0  0  0  1 


lo 

L>  C. 

-> 

h  2i 

’LW2 

llW2 

*W2  EW2 

0  0 

0  1 

0 

0 

0  1 

0 

0 

0  1 

Since  Xq,  io  Zd  orthonormal  unit  vectors,  then  the  inverse  matrix  in  the  above 
equation  may  be  computed  as 


-1 

X,J 

T!  PoiXoi 

i 

Xon 

Pox 

~T!  Poiy  oi 
i 

Xoy 

ycj 

Po7 

yox 

y.7 

y«i 

yci 

>01 

Poi 

*ox 

*«T 

*01 

Poi*ot 

0 

0 

0 

1 

0 

0 

0 

i 

1 

Finally,  it  is  the  controller’s  task  to  drive  the  hand  to  the  location 


n  s  a  £ 
00  0  1 


to  accom¬ 


plish  the  rendezvous. 


t 


BASECOORDINATtS 


X  ,n(t  ) 
— o  —  o 


Figure  k.  (a)  Position  and  Orientation  Vectors  of  the  Hand 
(b)  Euier  Angles  of  Orientation 


^  Expanded 
Obstacle 


Robot 


Figure  6.  Robot  Sliding  Around  the  Obstacle  with  No  Rotat 
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Kinematic  Control  Equations  for  Simple  Manipulators 

RICHARD  P.  PAUL,  senior  member,  ieee,  BRUCE  SlllMANO. 

AND  GORDON  E.  MAYER 

Abstract — The  basis  tor  all  advanced  manipulator  control  is  a  relation¬ 
ship  between  the  Cartesian  coordinates  of  the  end-effector  and  the  manipu¬ 
lator  joint  coordinates.  A  direct  method  for  assigning  link  coordinate 
systems  and  obtaining  the  end-effector  position  in  terms  of  joint  coordi¬ 
nates  is  reviewed.  Techniques  for  obtaining  the  solution  to  these  equations 
for  kinematically  simple  manipulators,  which  includes  all  commercially 
available  manipulators,  are  presented, 
j 

Introduction 

A  serial  link  manipulator  consists  of  a  sequence  of  mechanical 
I  links  connected  together  by  actuated  joints.  Such  a  structure 
‘  forms  a  kinematic  chain  and^  may  be  analyzed  by  methods 
developed  by  Denavit  and  Hartenberg  [10].  The  results  of  this 
analysis  are  the  matrix  equations  expressing  manipulator  end- 
effector  Cartesian  position  and  orientation  in  terms  of  the  jotnt 
coordinates.  These  equations  may  be  obtained  for  any  marupula- 
tor  independent  of  the  number  of  links  or  degrees  of  freedom. 

In  this  correspondence  we  first  review  the  method  of  obtaining 
these  equations  extending  the  procedure  of  assigning  coordinate 
frames  to  include  simple  manipulators  which  have  many  zero 
length  links  and  intersecting  joint  axes.  While  we  may  obtain 
these  kinematic  equations  for  any  manipulator  it  is  their  solution 
which  is  of  interest.  Given  a  desired  Cartesian  position  and 
orientation  of  the  manipulator's  end-effector  what  are  the  neces¬ 
sary  joint  coordinates?  While  there  is  only  one  end-effector 
position  corresponding  to  a  given  set  of  joint  coordinates,  there 
are  a  number  of  configurations  of  the  manipulator’s  links  all  of 
,  which  place  the  end-effector  in  the  same  position  and  orientation. 
Norm^ly  only  one  solution  corresponding  to  a  given  kinematic 
configuration  is  desired  (e.g.,  elbow  up  or  down,  etc.),  rather  than 
the  entire  set  of  solutions.  Frequently  the  solution  is  to  be 
embedded  in  a  real-time  servo  loop  and  only  a  very  minimum 
number  of  mathematical  operations  may  be  performed. 

When  the  manipulator  geometry  is  simple  and  well  understood 
a  trigonometric  solution  may  often  be  obtained  [1]-|3J,  (8),  [9]. 
However,  six-dcgrce-of-frccdom  manipulators  arc  sufficiently 
complex  that  the  direct  trigonometric  method  is  too  difficult  to 
apply.  We  present  a  method  of  obtaining  a  solution  to  the 
kinematic  equations  based  on  the  Hanenberg- Denavit  matrices 
from  which  the  solution  is  obtained  explicitly  in  the  case  of 
simple  manipulators.  The  existence  of  an  exph'cit  solution  to  the 
kinematic  equations  for  any  manipulator  is  of  great  importance 
in  evaluating  the  manipulator's  suitability  for  computer  control. 
Iterative  solution  tcchiiiqucs  can  involve  an  order  of  magnitude 
and  more  computation  lhan  an  e.xplicit  solution  Pieper  (5]  in  hjs 
thesis  considers  a  series  of  simple  manipulators  for  which  a 
closed-form  solution  is  obtainable.  It  is  to  these  "simple"  mani¬ 
pulators  that  the  solution  method  presented  in  this  correspon¬ 
dence  is  applicable.  We  have  solved  the  kinematic  cqualions  for 
all  commercially  available  manipulators  and  find  that  the  cqua¬ 
lions  can  be  readily  obtained  in  a  matter  of  hours. 
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Coordinate  Frames 

A  serial  link  manipulator  consists  of  a  sequence  of  bnks 
connected  together  by  actuated  joints.  For  an  n-dcgree-of-frcedoni 
manipulator,  there  will  be  n  links  and  n  joints.  The  base  of  the 
manipulator  is  link  0  and  is  not  considered  one  of  the  six  links. 
Link  1  is  connected  to  the  base  link  by  joint  1.  There  is  no  joint 
at  the  end  of  the  final  link.  The  only  significance  of  links  is  that 
they  maintain  a  fixed  relationship  between  the  manipulator  joints 
at  each  end  of  the  link  (7).  Any  link  can  be  characterized  by  two 
dimensions:  the  common  normal  distance  and  a„  the  angle 
between  the  axes  in  a  plane  perpendicul2u-  to  a„.  It  is  customary 
to  call  a„  “the  length”  and  a„  “the  twist”  of  the  link  (see  Fig.  1). 
Generally,  two  links  are  connected  at  each  joint  axis  (see  Fig.  2). 
The  axis  will  have  two  normals  connected  to  it,  one  for  each  link. 
The  relative  position  of  two  such  connected  links  is  given  by 
the  distance  between  the  normals  along  the  joint  n  axis,  and 
the  angle  between  the  normals  mea.sured  in  a  plane  normal  to  the 
axis.  and  0„  are  called  ‘the  distance"  and  “the  angle”  between 
the  links,  rc.spectively. 

In  order  to  describe  the  relationship  between  links,  we  will 
assign  coordinate  frames  to  each  link.  We  will  first  consider 
resolute  joints  in  which  0„  is  the  joint  variable.  The  origin  of  the 
coordinate  frame  of  link  n  is  set  to  be  at  the  intersection  of  the 
common  normal  between  joints  n  and  n  +  1  and  the  axis  of  joint 
n  4-  1.  In  the  case  of  intersecting  joint  axes,  the  origin  is  at  the 
point  t'f  intersection  of  the  joint  axes.  If  the  axes  are  parallel,  the 
origin  is  chosen  to  make  the  joint  distance  zero  for  the  next  link 
whose  coordinate  origin  is  defined.  The  z  axis  for  link  n  shall  be 
aligned  with  the  axis  of  joint  n  +  1.  The  x  axis  will  be  aligned 
with  any  common  normal  which  exists  and  is  directed  along  the 
normal  from  joint  n  to  joint  n  +  1.  In  the  case  of  intersecting 
joints,  the  direction  of  the  x  axis  is  parallel  or  antiparallel  to  the 
vector  cross  product  Notice  this  condition  is  also 

satisfied  for  the  x  axis  directed  along  the  normal  between  joints  n 
and  n  T  1.  For  the  nth  revolute  joint  when  x„_,  and  x,  are 
parallel  and  have  the  same  direction,  is  at  its  zero  position. 

In  the  case  of  a  prismatic  joint  the  distance  d„  is  the  joint 
variable.  The  direction  of  the  joint  axis  is  the  direction  in  which 
the  joint  moves.  Although  the  direction  of  the  axis  is  defined, 
unlike  a  revolute  joint,  its  position  in  space  is  not  defined  (see 
Fig.  3).  In  the  case  of  a  prismatic  joint  the  length  a,  has  no 
meaning  and  is  .set  to  zero.  The  origin  of  the  coordinate  frame  for 
a’  prismatic  joint  is  coincident  with  the  next  defined  link  origin. 
The  z  axis  of  the  prismatic  hnk  is  aligned  with  the  axis  of  joint 
n  +  1.  The  x„  axis  is  parallel  or  antiparallel  to  the  vector  cross 
product  of  the  direction  of  the  prismatic  joint  and  z„.  For  a 
prismatic  joint,  we  will  define  its  zero  position,  with  d,  =  0,  to  be 
when  x„_i  and  x„  intersect.  With  the  manipulator  in  its  zero 
position,  the  positive  sense  of  rotation  for  revolutc  joints  or 
displacement  for  prismatic  joints  can  be  decided  and  the  sen.se  of 
the  direction  of  the  z  axes  deternuned. 

The  origin  of  the  base  link  (zero)  will  be  coincident  wh  the 
origin  of  link  1.  If  it  is  desired  to  define  a  different  reference 
coordinate  system  then  the  relation.ship  between  the  reference 
and  base  coordinate  systems  can  be  described  by  a  fixed  homoge¬ 
neous  transformation  (6).  At  the  end  of  the  manipulator  the  final 
displacement  or  rotation  occurs  with  respect  to  Zj.  The 
origin  of  the  coordinate  system  for  link  6  is  chosen  to  be 
coincident  willi  that  of  the  link  5  coordinate  system.  If  a  tool  or 
end-effector  is  used  whose  origin  and  axes  do  not  coincide  with 
the  coordinate  system  of  link  6,  the  tool  can  be  related  by  a  fixed 
homogeneous  transformation  to  link  6. 

Having  a.s.signcd  coordinate  frames  to  all  links  according  to  the 
preceding  scheme,  we  can  establish  the  relationship  between 
succe.ssivc  frames /i  —  1,  n  by  the  following  rotations  and  transla¬ 
tions. 

Rotate  about  z„  ,,  an  angle  9„. 

Translate  along  z„  ,,  a  distance  d„. 


Fig  Length  a,  and  lui&t  a.  of  a  link. 


Translate  along  rotated  x„_  ,  =  x„,  a  length  a„. 

Rotate  about  x„,  the  twist  angle  q„. 

This  may  be  expressed  as  the  product  of  four  homogeneous 
transformations  relating  the  coordinate  frame  of  link  n  to  the 
coordinate  frame  of  link  n  —  This  relationship  is  called  an  A 
matrix: 

’  ce  -seca  sesa  ace] 

j  ^  SB  CBCtt  -CBSa  aSB  ! 

'^"0  Sa  Ca  d  \  ^  ^ 

.0  0  0  1  J 

where  S  and  C  refer  to  sine  and  cosine,  respectively.  For  a 
prismatic  joint  the  A  matrix  reduces  to 

CB  -SBCa  SBSa  O' 

j  ^  SB  CBCa  -CBSa  0 

"0  5a  Ca  d  '  ^  ' 

.0  0  0  1. 

Once  the  link  coordinate  frames  have  been  assigned  to  the 
manipulator  the  various  constant  link  parameters  can  be  tabu¬ 
lated:  d,  a,  and  a  for  a  link  following  a  revolute  joint  and,  B  and 
a  for  a  link  following  a  prismatic  joint.  Based  on  these  parame¬ 
ters,  the  constant  sine  and  cosine  values  of  a  may  be  evaluated 
and  the  values  for  the  six  A,  transformation  matrices  determined. 

Kinematic  Equations 

Having  assigned  coordinate  frames  to  a  manipulator  it  is 
possible  to  obtain  the  Cartesian  position  and  orientation  of  the 
manipulator  e'nd-cffector  when  given  the  joint  coordinates. 

The  description  of  the  end  of  the  manipulator,  fink  coordinate 
frame  6,  with  respect  to  link  coordinate  frame  n  —  1  is  given  by 
(/„  where  ‘ 

=  '■!«• '^n+i  *  ••  (3) 

The  end  of  the  manipulator  with  respect  to  the  base,  known  as 
Tj.  is  given  by  f/,: 

=  Ai  •  A2  •  A}  •  A  ^  ^  A  Af,.  (4) 

If  the  manipulator  is  related  to  a  reference  coordinate  frame  by  a 
transformation  Z  and  has  a  tool  attached  to  its  end  described  by 
we  have  the  description  of  the  end  of  the  tool  with  respect  to 
the  reference  coordinate  system  described  by  X  as  follows  (4). 

(5) 

In  Fig.  4  the  PUMA  arm  (Unimatc  600  Robot)  is  shown  with 
coordinate  frames  assigned  to  the  links.  The  parameters  are 
.shown  in  Table  1. 
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where  S23  refers  to  sinfflj  +  6y)  and  C23  refers  to  cos(fl2  +  ®j). 


U,=  A,U,=  T,= 


Fig.  4.  PUMA  manipuialor. 


TABLE! 

Link  Parameters  for  PUMA  Arm 


Joint 

a- 

tf” 

tf  21 

Range 

1 

-90'’ 

tf, 

0 

0 

tf,:-F/-160” 

2 

0 

«2 

0 

tf2:-F45  ->  -225” 

3 

■  90” 

93 

tfj  a, 

tf3;225”  -  -45” 

4 

-90” 

^4 

2(4  0 

«4-.+/-170* 

5  * 

90” 

64 

0 

0 

♦j.-F/-135” 

6 

a,=  17.000 
/,  =  4  937 

0 

flj  =  0.75 
4/4=  17.000 

9* 

0 

0 

tf4:+/-170” 

the  A  matrices,  starting  at  link  6  and  working  back  to  the  base, 
for  the  PUMA  arm  are 


U,=  A,U,  =  \ 


-QS, 

-SiS, 

Q 

0 


U,  =  A,U,= 


-C,CiS,-S,Q  QSi 
S,QC,+  C,S^  -S^CjSj+QQ  S,Si 

0  00 


—  C)[C23(C4C5Q  548(1)  52355Q] 

-S,[S4C5Q+ QSJ  (18) 

^y~  5i[C2j{C4CjC^~  848^)  —  52355^1 

+  C,[S4C5Q+ C4SJ  (19) 

Jij  —  ~ 82^(040 jC(i~  848^)  ~  C23S5C4  (20) 

—  C|[  — C23(C4C5S4  +  54C(,)  +  5235554) 

-5,[-54C55;,+ C4QI  (21) 

Oy—  5|  [  ~  C23(  C4C5  5^  +  54C4)  +  5235554] 

+  C,[-54C554+ C4(^l  (22) 

Oj  —  523(C4C554  +  54C^)  +  C23  55^  (23) 

Oj,  =  Ci(C23C455  +  523C5)  ~  5|5455  (24) 

=  5|(C23C4  55  +  523C5)  +  C|5455  (25) 

o,  =  —  523C455+  C23C5  (26) 

Px  ~  ^|(‘^4^23  °i^2i  ■*"  “2Q)  “  (22) 

p,  =  5,(^4523  +  O3C23  +  O2C2)  +  Cydy  (28) 

Pj  ~  ~(~^4C23  +  ^3  523  +  272^2)-  (2^) 

In  order  to  compute  the  right  hand  three  columns  of  7^,  we 
require  12  transcendental  function  calls,  34  multiplies,  and  16 
additions.  The  first  column  of  Tf,  can  be  obtained  as  the  vector 
cross  product  of  the  second  and  third  columns. 

If  the  joint  coordinates  are  given,  the  position  and  orientation 
of  the  hajjd  are  obtained  by  evaluating  these  equations  to  obtain 
7^.  The  position  and  orientation  of  a  tool  with  respect  to  a  base 
coordinate  frame  can  now  be  obtained  from  (5). 

Solution 

In  order  to  control  the  manipulator,  we  are  interested  in  the 
reverse  problem,  that  is,  given  X  in  (5),  what  are  the  correspond¬ 
ing  joint  coordinates? 

We  may  first  obtain  7'4  from  (5)  as 

T(,=  Z-'*  X»  (30) 

and  then  the  traditional  approach  is  to  solve  the  matrix  equation 


2'6=^I*^2 


>  A4*  A^*  A(, 


Cj(C4CjC(,  ~  848(1)  —  8y8^C(, 

Uj  =  AjU4  =  ~  ^*^6) 

■^4^5  C»  8(, 

0 


~ C3 ( C4C5 54 -1-  54C4)  +  8y8^8(,  C3C455  -F  53C5  ^48)  +  U3C3 

~8){C4C^8^  54C4)  ~  Cj8}8f,  53C455  ~  C3C5  ~d4Cy  -F  O353 

"'^4^556+^4(14  848^  2/3 

0  0  1  . 


U2  =  A2U,= 


^23  (  C4C5  C4  848(1)  82)  8^  C4 

S:y(C4QC(,-  848,)  +  C2i8iQ 
S4CiC(,+  C454 
0 


~  ^4(^6 )  +  82^8^8^  C23C45^  -F  523C5 

+  ■S^Q)  ~  C2-i8f8(,  523C455  —  C23C5 

“54C5.94 -F  C4C)5  848^ 

0  0 


^4822  +  OiC2yO-^2 
“^4(223  +  O3523  +  O282 

^3 

1 
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-.1 

-.1 


L  *. 


it 


where  7^  is  given  numeric  values.  With  numeric  values  assigned 
to  the  elements  of  7^,  the  required  values  of  82,  Oj,  8^,  8^,  and 
8^  can  be  obtained  by  simultaneously  solving  (18)-(29).  This 
approach  is  difficult  for  the  following  reasons;  the  equations  are 
transcendental;  we  will  need  both  the  sine  and  cosine  in  order  to 
determine  angles  uniquely  and  accurately;  the  manipulator  ex¬ 
hibits  more  than  one  solution  for  a  given  position;  and  we  have 
twelve  equations  in  six  unknowns. 

There  are,  however,  six  other  matrix  equations  obtained  by 
successively  premultiplying  (31)  by  the  A  matrix  inverses; 


U2 

A2'*Ar'*n=  7/3 

A2^.A2'»Ar'*T^=  74 


A^  ' •A^  ^  *  A2 


-/<r‘‘7;=  7/3 


Aj  ^  •A^  '  •Ay  '  •Ay  '*^1  '  •T(,=  74. 


(32) 

(33) 

(34) 

(35) 

(36) 


/If*  *74=  Uy. 
The  left  side  of  (38)  is  given  by 


Cl 

5, 

0 

o' 

■"4 

Ox 

Ox 

Px 

0 

0 

-1 

0 

n. 

Oy 

°y 

Py 

-5, 

c, 

0 

0 

"4 

0, 

0, 

P. 

0 

0 

0 

1 . 

.  0 

0 

0 

1  . 

■T,= 


where 


/ii(")  /ii(<*) 

f\\{o) 

/ii(p) 

/l2(")  /l2(<*) 

/12(a) 

fnip) 

(40) 

/l3(")  7|3(o) 

/13(a) 

/|3(P) 

0  0 

0 

1 

/ii  —  CyX  -b  S\y 

(41) 

f\2  = 

(42) 

/i3=  -S,x  +  C^y 

(43) 

All  the  elements  on  the  right  side  of  (45)  are  functions  of  8y.  8yy, 
^4,^4,^},  and  8^  except  for  element  34.  We  may  equate  the  34 
elements  to  obtain 

Aj(p)  =  dj  (46) 

or 

-s,p^+ c,p^=  -dy.  (47) 

In  order  to  solve  equations  of  this  form  we  make  the  following 
trigonometric  substitutions; 

(48) 


where 


Px  =  r  cos  4> 

Py  —  r  sin  4> 

r=-^{pl>^pir 


(49) 

(50) 

(51) 


The  matrix  elements  of  the  left  sides  of  these  equations  are 
functions  of  the  elements  of  7^  and  of  the  first  n  —  1  joint 
variables.  The  matrix  elements  of  the  right  hand  sides  are  either 
zero,  constants,  or  functions  of  the  nth  to  6th  joint  variables.  As 
matrix  equality  implies  element  by  element  equality  we  obtain  12 
equations  from  each  matrix  equation,  that  is,  one  equation  for 
each  of  the  components  of  the  four  vectors  n,  o,  a  and  p. 
Equating  elements  of  these  matrix  equations  frequently  results  in 
equations  yielding  joint  variables  explicitly.  We  will  illustrate  the 
various  forms  of  these  equations  by  developing  the  equations  for 
the  PUMA  arm. 

If  we  premultipy  (31)  by  ,4,“  *  we  obtain 

A^  ^•T^—Ay^Ay^A^^Aj^A^ 


(37) 

(38) 


As  either  the  numerator  or  denominator  of  (51)  can  be  zero  we 
will  use  the  arctangent  function  of  two  arguments  to  obtain 
values  of  4>.  This  arctangent  function  uses  the  sign  of  the  numera¬ 
tor  and  denominator  to  determine  the  correct  quadrant  for  the 
resulting  angle  and  is  defined  over  the  range  —  w  <  <#>  <  w.  Sub¬ 
stituting  for  p^  and  Py  in  (47)  we  obtain 

S<t>C8^  -  C<t>S8^  =  dy/r  (52) 

with 

Q<dy/r<\. 

Equation  (52)  reduces  to 

S{^-8,)  =  dy/r  (53) 

with 

0  <  -  »,<  w. 

We  may  obtain  the  cosine  as 


C(^ -(?,)=  ±y/l  -  {dy/rf 


(39) 

The  inverse  of  a  homogeneous  transformation  is  simple  to  obtain 
(see  Appendix  I)  and  the  product  of  these  two  matrices  is 


(54) 

where  the  minus  sign  corresponds  to  a  left-hand  shoulder  config-’ 
uration  of  the  manipulator  and  the  plus  sign  corresponds  to  a 
right-hand  shoulder  configuration.  Finally, 


8,  =  tan' 


tan 


(55) 


and  .t,  y,  and  z  refer  to  components  of  the  vectors  given  as 
arguments  to  and  f^y,  for  example 

/ii(")  =  C,n^+ (44) 

The  right  side  of  (38)  is  obtained  frorh  (16)  and  is  given  by 


Having  determined  8^  the  left  side  of  (38)  is  now  defined. 
Whenever  we  have  the  left  side  of  one  of  (32)-(36)  defined,  we 
examine  the  right  side  for  elements  which  are  a  function  of 
individual  joint  coordinates.  In  the  case  of  the  PUMA  arm,  as 
with  any  arm  with  two  or  more  joint  axes  parallel,  the  7^  matrix 
is  expressed  in  terms  of  sums  or  differences  of  the  angles  relating 
to  the  parallel  axes.  In  order  to  solve  the  kinematic  equations,  the 
sum  or  difference  of  the  angles  must  be  determined  before  the 
angles  themselves  can  be  found.  In  addition  the  solution  for  these 
sums  of  angles  involves  the  sum  of  the  squares  of  two  equations. 
Such  is  the  case  in  order  to  solve  for  8y  and  fl,.  The  14  and  24 
elements  of  (38)  are 

OyCy  =  C\P^+  S\Py  (56) 

~  <^4(^1^'^  OySy  =  —pi  (57) 


7/,= 


7-23(747474“  ^4^t>)  “  ^23^574 

^23(747^374  “  S^S^)  +  CyySyCf, 

^47474  7454 
0 


'  ^23(7474^6  S474)  +  SyySjS^  C23C4S5+  SyyCj  ‘74^23'*'  ‘*37-23  **27^2 

■^23(747^35^6  54Cj)  ~  CyySyS^  SyyC^Sy  ~  ~^4^2i  ^2^2 

“  S4CJ  Sj -f  ClQ  ^4^i  ‘^3 

0  0  1 


(45) 
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C,;»,  +  =/u,  (58) 

-P:=fi7r-  (59) 

Squaring,  adding,  and  simplifying: 

fup  +  fnp~  d*—  a]  —  al  =  +  la^a^C^.  (60) 

Since  the  left  side  is  known  and  the  only  variables  are  S3  and  Cj, 
this  equation  is  of  the  form  of  (47).  It  can  be  solved  to  yield 

^3=arctan-^^ - arc  tan — ,  ^  =  (61) 


d  =  /m,  +  fi\p  -dl-a]-  al 
e  =  Aaloj  +  Aaldl  (constant). 
Evaluating  the  elements  of  (33)  we  obtain 

/2i(")  hiio)  filial)  h\{p)-Oi 
fiiia)  /n(p) 
/23(")  fiiio)  fiM  fn{p) 
.0  0  0  I  . 


With  the  left  side  of  (68)  now  defined,  we  check  the  right  side  for 
functions  of  single  variables.  The  13  and  23  elements  give  us 
equations  for  the  sine  and  cosine  of  6^  if  sinf^,)  is  not  zero. 
When  sin  =  0,  ^5  =  0  and  the  manipulator  becomes  degenerate 
with  both  the  axes  of  joint  4  and  joint  6  aligned.  In  this  state  it  is 
only  the  sum  of  0^  and  0^  which  is  significant.  If  0^  is  zero  we  are 
free  to  choose  any  value  for  0^.  The  current  value  is  frequently 
assigned: 

C^Si=  S^ay')  —  S-iiO,  (80) 

+  C,a.  (81) 


^4  =  tan' 


-5,a,+  C,a, 


=  U,  (64) 


f,^=C,{C^x  +  S,y)-S,z  (65) 

hi=  -S^iC,x  +  S^y)-C,z  (66) 

/j3= -S,x  +  C,j».  (67) 

Since  this  yields  nothing,  we  evaluate  (34)  as 

f}M  h\{o)  /„(a)  fi\{p)-  a^Ci- 

/32(«)  hiio)  /32(a)  fi2{p)  +  dj 
/33(")  /33(a)  f}jia)  fnip)~‘^i^i 

.0  0  0  1 

(68) 

hx=C2,{C,x  +  S,y)-S„z  (69) 

fn=  -SiX  +  C^y  (70) 

/,3=S23(C,x  +  S,y)  +  Cj3Z  (71) 

equaling  the  14  and  34  terms  we  obtain 

Q3/1IP  ~  '^23Pi  “  ^303  +  03  (72) 

■^23/1  ^i3Pi~  dt  +  a2Sj.  (73) 

Since  C23  and  are  the  only  variables,  we  can  solve  the  above 
equations  simultaneously  to  yield: 

”  /,V  +  a’  '  ’ 

”  /,>.,*  ri  '  ' 

where 

*v,  =  ajCj  +  aj  (76) 

W2  =  d^+a2S2  (77) 


64=64+180“  if6j<0. 

Evaluating  the  elements  of  (35)  we  obtain 

/4i(")  /41(a)  /41(a)  0  CjQ  —C^Sf,  Sj 

/42(a)  /42(a)  0  _  SjCf,  ~S^Si,  ~Ci 

/43(a)  /43(a)  /43(a)  0  Sj  Q  0 

.0  0  0  ij  L  0  0  0 


/4i=  C4[Cj3(C,x  +  Sry)  -  Sjjz)  +  54(-5,x  +  C,y]  (85) 

/ai  ~  ~^ii{CiX  +  Siy  )  —  €22^  (86) 

/43=  -54[C23(C,x  +  s,y)-  Sj,z]  +  C4[-S,x  +  Co  ). 

(87) 

From  the  right  side  of  (84),  we  can  then  obtain  equations  for  5}, 
C5,  Sf,  and  Q  by  inspection.  When  both  sine  and  cosine  are 
defined  we  obtain  a  unique  value  for  the  joint  angle.  We  obtain  a 
value  for  65  by  equating  the  13  and  23  elements  of  (84): 

,  Si  =  C4[C23(C|t2j,  +  .5|a,,)  -  .523a,]  +  ■54[-S,fl,  +  C,a,] 


Q  “  '^23(Clfl;,  +  SxOy)  +  C23fl, 


and  obtain  65  as 


therefore 


.  ’*'2/11,- w.p, 

6ji  =  arctan  -  j  — : - 

*^l/llp+  *^2  7', 


_  _ I  C4[C23(C|a;,  +  ,?ia^)  53322,]  +  54[  .5|a,  +  C|a,.] 

'  S2,(C,a,+  S,aJ  +  C23a, 

(90) 

While  we  have  equations  for  both  and  Q,  the  equation  for  Sj 
is  in  terms  of  elements  of  the  first  column  which  involves  the  use 
of  the  n  vector  of  7^.  The  n  vector  of  7^  is  not  usually  made 
available  as  it  represents  redundant  information.  It  can  always  be 
computed  by  the  vector  cross  product  of  the  o  and  a  vectors.  By 
evaluating  the  elements  of  (36)  we  can  obtain  equations  for  Sj 
and  Q  as  a  function  of  the  a  vector: 

/5i(")  fii(o)  0  0  Fq  -Ss  0  0' 

/52(«)  fii(o)  0  0  _  5^  Q  0  0 

/53(")  fi^io)  10  0  0  10 

0  0  OiJlO  0  01. 
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where 

/s.=  Q{C4[Cj3(C,x  +  S,y)  -  S^yz]  +  541-5, X  +  C,y]) 

+  Sy[-Syy{C,x  +  S,y)-Cyyz)  (92) 

/«=  -54(Cj3(C.x  +  S^y)  -  5j3^)  +  C4(-5,x  +  C,y]  (93) 

fiy  —  +  ^ly)  ~  ]  +  5'4l— 5|X  +  Cj^)} 

+  C3{5j3(C,x  +  5,>^)  +  Cj3z}.  (94) 

By  equating  the  12  and  22  elements  we  obtain  expressions  for  5^ 
and  Q: 

S(,=  —  C3{C4[C23(C,Oj,+  5,Oj,)  —  523O,]  +  54[  — 5,Oj, 

+  C.oJ  }  +  53  { 523( CyO,  +  5.0^  )  +  C23O, )  (95) 

(96) 

We  obtain  an  equation  for  ^4  as; 


Its  inverse  is  given  by 

'  1.  ly  I,  -pi' 

J--1_  '”x  '^y  ~pfn 

"x  ">■  "»  -p-n  ^ 

.0  0  0  1  . 

where  the  terms  of  the  right-hand  column  are  obtained  using 
vector  dot  product.  That  (99)  represents  the  inverse  is  easily 
verified  by  forming  the  matrix  product  and  checking  that  the 
result  is  an  identity  matrix; 


(100) 


_  -I  Q  {^4[^23(^l^x  ^l^y)  ^23^x]  ^4[  ■S^l^x  ^l^y]  )  ‘^3  {‘^23(^1  ^x  ^l^^)  ) 


=  tan 


^4[6^23C^1®x"^  ^23®i]  Q[  ■^l®x"*"Q®y] 


Even  in  the  case  where  0^  is  undefined  because  the  manipulator 
configuration  is  degenerate,  once  a  value  is  assigned  to  ^4  the 
correct  values  for  Oy  and  ^4  are  determined  by  jthese  equations. 
This  solution  corresponds  to  16  transcendental  function  calls,  38 
multiplies,  and  25  additions. 


CyOy] 

■  II 

1  m 

!  m 

ml 

mm 

m/I 

n-l 

n-m 

nn 

.  0 

0 

0 

Extension  to  Other  Manipulators 

This  solution  technique,  demonstrated  with  the  PUMA  mani¬ 
pulator,  is  valid  for  kinematically  simple  manipulators,  including 
all  commercially  available  mam'pulators  for  wUch  solutions  have 
been  obtained.  There  are,  however,  some  manipulators  whose 
configurations  mandate  a  slightly  different  approach  to  the  solu¬ 
tion.  In  the  case  of  a  manipulator  with  an  offset  at  the  hand,  the 
problem  was  inverted  and  the  solution  to  the  kinematic  problem 
to  position  the  base  at  7^"*  was  solved. 

There  are  two  common  pitfalls  in  obtaining  solutions  which 
should  be  avoided.  One  of  these  is  division  by  the  sine  or  cosine 
of  an  angle.  The  other  is  not  maximizing  the  use  of  common 
expressions.  For  example,  after  solving  for  6^  from  (82),  a  possi¬ 
ble  method  to  determine  By  would  be  to  equate  the  2, 3  and  3, 3 
elements  of  (68).  In  order  to  do  this,  the  2,3  element  (S4S}) 
would  have  to  be  divided  by  S4.  This  leads  to  inaccuracy  when  54 
is  near  or  equal  to  zero.  By  extending  the  method  one  more  step 
and  premultiplying  by  Aj  both  problems  were  avoided. 

Summary 

We  have  reviewed  the  method  of  assigning  coordinate  frames 
to  the  links  of  a  manipulator.  In  terms  of  these  coordinate  frames 
the  kinematic  equations  can  be  developed  in  a  straightforward 
manner.  These  equations  can  be  obtained  for  any  manipulator.  If 
the  manipulator  is  kinematically  “simple,”  the  solution  to  the 
kinematic  equations  can  be  obtained  in  a  very  straightforward, 
error-free  manner. 

Appendix  1 

Given  a  homogeneous  transformation  represented  by  four  vec¬ 
tors  /,  m,  H,  and  p 


As  the  three  vectors  f,  m,  and  n  are  orthogonal  we  have 

/•/=  mm  =  «•/!=  1  (102) 

and 

l-m  =  In  =  nm  =  0  (103) 

and  thus  (101)  reduces  to  an  identity  matrix. 
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ABSTRACT 

Industrial  robots  are  serial  link  manipulators  whose  dynamic  charac¬ 
teristics  are  highly  nonlinear.  By  controlling  each  link  or  joint  individu¬ 
ally,  it  is  possible  to  employ  the  conventional  technique  to  design  linear 
feedback  controllers  for  the  robot.  The  gravitational  force  and  force  in¬ 
teractions  between  joints  are  suppressed  by  precalculated  feedforward  com¬ 
pensation.  To  ease,  the  computational  burden,  the  compensating  signals  are 
either  approximated  or  their  computational  formulas  are  simplified. 

I.  INTRODUCTION 

The  number  of  joints  of  industrial  robots  commercially  available  today 
varies  from  three  to  seven.  Typically  they  have  six  joints,  giving  six  de¬ 
grees  of  freedom,  with  a  gripper  which  is  referred  to  as  a  hand  or  an  end 
effector.  Figures  1  and  2  show  the  commercially  available  Cincinnati  Mi  la- 
cron  Model  T3,  and  Unimation  PUMA  600.  Figure  3  shows  a  Stanford  manipula¬ 
tor  C13  which  is  also  an  industrial  robot  existing  among  research  institutes 
in  the  United  States.  Each  joint  of  these  robots  is  driven  hydraulically, 
pneumatically  or  electrically  with  a  feedback  control  loop.  As  an  example, 
a  block  diagram  for  a  joint  control  of  the  Stanford  manipulator,  which  has  a 
permanent  magnet  motor  drive  C23,  is  shown  in  Figure  4.  It  has  an  optical 
encoder  for  positional  feedback  with  a  tachometer  feedback  for  damping. 
Thus,  an  industrial  robot  is  a  positioning  device  in  that  each  of  its  joints 
★Supported  by  NSF  Gran-t  DAR  (APR  77-14533)  . 
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has  a  positional  control  system. 

II.  CARTESIAN  AND  JOINT  COORDINATES 

In  reality  a  robot  task  is  naturally  specified  in  terms  of  its  hand  in 
Cartesian  coordinates.  It  consists  of  position,  described  by  a  position 
vector  £(t),  and  orientation,  described  by  a  unit  approach  vector  £(t)  and  a 
unit  sliding  vector  s^(t),  as  indicated  in  Figure  5(a).  All  these  vectors 
are  defined  with  reference  to  the  base  coordinates.  For  convenience,  a  unit 
normal  vector  is  defined  as  ^(t)  =  £(t>  X  £(t)  where  X  denotes  the  "cross 
product."  Thus  the  state  of  hand  at  time  t  in  Cartesian  coordinates  can  be 
represented  by  a  A  by  4  hand  matrix 

£(t)  £(t)  £(t)  £(t)“j 

0  0  0  1 


H(t)  = 


The  last  row  in  ^(t)  is  added  for  the  convenience  of  future  computation  us¬ 
ing  homogeneous  transformation  C33.  The  orientation  may  also  be  defined  in 
terms  of  Euler  angles  with  respect  to  the  base  coordinates.  Initially  at 
t  =  tg,  let  ri(tQ),  and  align  with  x^,  ^  and  respectively, 
as  shown  in  Figure  5(b).  Any  orientation  [jT^(t)  £(t)  _a(t)3  may  be  obtained 
by  a  rotation  of  y  radians  about  so  that  £(tg)  aligns  with  £(t^);  then  a 
rotation  of  6  radians  about  £(t^)  so  that  £(tg)  =  £(t^)  aligns  with  £(t), 
and  finally  a  rotation  of  a  radians  about  £(t)  to  obtain  the  required  £(t) 
and  s^(t).  This  is  equivalent  to  rotate  the  j^ltg)  £^tg)  i.^Tg)J  coordi¬ 
nate,  which  aligns  with  ^  originally,  o  radians  about  then  6 
radians  about  and  finally  Y  radians  about  ^  again.  The  relationship  is 


thus 
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n(t)  = 


cos  a  cos  0  cos  Y  “  sin  a  sin  y' 
cos  a  cos  6  sin  y  +  sin  a  cos  Y 
-cos  a  sin  3 


(2) 


s(t)  = 


-sin  a  cos  6  cos  y  -  cos  a  sin  y* 
-sin  a  cos  6  sin  y  +  cos  a  cos  Y 
sin  a  sin  0 


(3) 


a(t)  = 


sin  0  cos  Y 
sin  0  sin  y 
cos  0 


(4) 


Consequently  the  state  of  hand  at  time  t  in  Cartesian  coordinates  with 
reference  to  the  base  coordinates  may  also  be  represented  by  a  6-dimensiona L 
£(t)  ^(t)  where  Q.(t)J  =  C“  ®  yD  and  (  )  =  transpose  of 


(  ). 


The  hand/  however^  is  driven  by  the  actuators  at  the  joints.  Intui- 
tively/  if  all  the  joint  displacements  are  given/  the  position  and  orienta¬ 
tion  of  the  hand  are  determined.  Let  n  be  the  number  of  joints.  For 


i  =  1/  2/  .../  n,  let  q.  be  the  displacement  of  the  i-th  joint  with  respect 


to  its  own  reference  point.  Then/  for  any  given  robot  with  known  geometri¬ 
cal  dimensions/  there  is  a  relation 


[£(t)  lit)*]'  =  f_[q^/  •••/  qj 


(5) 


where  f(*)  is  a  6  by  1  vector  valued  function.  This  relation  is  known,  but 
almost  always  nonlinear  which  complicates  the  problem  C4].  Since  in  reali¬ 
ty,  one  specifies  |£(t)  in  Cartesian  coordinates  and  desires  to 
determine  the  corresponding  jq^,  •••,  qj  so  that  one  may  command  the  joint 
actuators  to  comply  with  the  specification  in  Cartesian  coordinates.  The 
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solution  requires  the  inverse  vector  function  f  (•)  of  n-dimension.  This 


•  ■  ^  ^  •u  •/ 


solution^  if  it  can  be  founds  may  not  be  unique.  for  the  commerciaLLy 
available  robots  in  operation^  n  is  usually  either  5  or  6.  The  geometrical 
configuration  of  these  robots  with  proper  definitions  and  ranges  of  en¬ 

ables  one  to  obtain  a  unique  solution  of  equation  (5)  C4]. 

Knowing  the  transformation  of  position  in  Cartesian  and  joint  coordi¬ 
nates^  it  is  ready  to  examine  the  following  simple  task.  As  shown  in  Figure 
6,  the  robot  is  required  to  go  to  the  bolt  magazine  to  fetch  a  bolt^  and 
place  it  on  top  of  the  bolt  hole  on  the  beam.  Intuitively  one  expects  that 
the  robot  hand  travels  along  the  path  of  straight-line  segments,  as  indicat¬ 
ed  in  Figure  6,  so  that  it  will  reach  the  bolt  first  and  then  arrive  at  the 
beam.  The  question  is  how  would  one  control  the  joint  actuators  to  accom¬ 
plish  the  goal?  Before  one  arrives  with  an  answer,  one  may  examine  the  fol¬ 
lowing  two  possible  specifications: 

<a)  Is  the  work  space  free  from  obstacles? 

<b)  Must  the  hand  follow  the  specified  path? 

The  answers  to  each  of  these  two  questions  could  be  either  yes  or  no.  They 
combine  to  form  four  different  classes  of  problems  as  follows: 


Is  the  work  space  free  from  obstacles? 


M 


% 

k>:- 


i* 


I 
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No 

iii 

• 

1 

I 
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Path  I 
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Path  Planning 
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On-line  Path 

i 

1 

I 

I 

T  racking 

• 

■'•v 

Only  the  problems 

of  Classes  1 

and  2 

will  be  analyzed 

briefly  in  the  follow- 

•  •  4 
•  » 

ing  sections. 

si 

III.  POSITIONAL 

CONTROL 

If  there  is 

no  path  constraints 

and  if  the  work 

space  is  free,  then  the 

controllers  only  have  to  make  sure  that  the  hand  passes  through  all  the 
specified  corner  points  of  the  path.  It  involves  the  coordinates  transfor¬ 
mation^  which  computes  the  corresponding  joint  coordinates  qj  of 
the  specified  corner  points  in  Cartesian  coordinates  by  means  of  f  and 


then  positionally  control  the  robot  in  joint  coordinates  from  point  to 
point.  In  practice,  a  positional  servo  is  used  for  each  joint.  If  the 


robot  is  allowed  to  move  only  one  joint  at  a  time  alternatively  by  locking 
all  the  other  joints,  then  each  of  the  joint  controllers  is  very  simple. 
When  the  number  of  joints  in  simultaneous  motion  is  more  than  one,  force  in¬ 
teractions  among  the  joints  create  couplings  which  complicate  the  control 
system.  These  topics  will  be  discussed  as  follows. 

A.  Single-Joint  Controller 

In  the  following  discussion,  the  robot  is  considered  to  have  a  rigid 

body  structure.  Refer  to  Figure  7(a),  the  schematic  representation  of  an 

actuator-gear-load  assembly  for  a  single  joint,  in  which 

2 

J  =  actuator  inertia  of  one  joint  (oz-in-sec  /  rad), 
a  ' 

=  manipulator  (robot)  inertia  of  the  joint, 

=  inertia  of  the  load, 

=  damping  coefficient  at  actuator  side  (oz-in-sec  /  rad), 

=  damping  coefficient  at  load  side, 
fjji  =  average  friction  torque  (oz-in), 

Tg  =  gravitational  torque, 

Tju  =  generated  torque  at  actuator  shaft, 

=  load  torque, 

=  angular  displacement  at  actuator  shaft  (radians), 

=  angular  displacement  at  load  side. 

Let 


N^,Ng  =  number  of  teeth  of  the  gear  at  the  actuator, 
shaft  and  load  shaft,  respectively, 

r  ,r  =  pitch  radii  of  the  gears  at  the  actuator 
shaft  and  load  shaft,  respectively. 


I  I  ■  ,  9  mi  ■  ■■■n-i 


I*.  • 

Q 
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n  =  r  /r  =  N  /N  <  1. 
tn  s  m  s  — 


(6) 


P.  ' 


1^* 


I 


is  the  gear  ratio.  As  indicated  in  Figure  7(b),  the  force  F  is  transmitted 
from  the  actuator  to  the  Load  at  the  contacting  point  of  the  mating  gears. 
Thus 


=  equivalent  Load  torque  at  actuator  shaft 
=  Fr 


and 


T,  =  Fr 
L  s 


which  Leads  to 


■^1 

L  L  ms 


or 


Tj_  =  n 


From  Figure  7Cc), 


0  =  2ir/N 

m  m 


0^  =  2ir/N 
s  s 


so  that 


0„  =  0  N  /N  =  n  0 
s  m  m  s  s 


(7) 


(8) 


(9) 


(10) 


(11) 


(12) 


By  taking  time  derivatives  on  both  sides  of  (12),  one  obtains  the  relations 
between  angular  velocities  and  accelerations  as 
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e  =  n  e 
s  m 


(13) 


e'  =  n  0  (14) 

S  ID 


From  Figure  7  (.a) ,  it  is  seen  that  the  load  torque  is  required  to  overcome 
the  Load  inertia  effect  J  ¥  and  damping  effect  B  6  .  Using  D'Alembert's 

L  S  L  ^ 

principle  "Z  torque  =  Z(J9)",  one  obtains 


\  -  Vs  =  \®s 


(15) 


Apply  the  same  principle  at  the  actuator  shaft  to  yield 


T  -  T,  -  B  0  =  (J  +  J  )0 

m  L  mm  a  mm 


(16) 


If  we  wish  to  express  the  torque  relation  at  the  actuator  shafts  first  sub¬ 
stitute  (13)  and  (14)  into  (15)  and  then  combine  the  result  with  (9)  to  ob¬ 
tain 

T,'  =  n^(J,  0’  +  B,  0„)  (17) 

L  L  m  L  Jn 

Combining  (16)  and  (17)  yields 

=  (J,  +  J™  +  +  n^B,  )0„  (18) 

m  am  Lm_m  Lm 

where  =  (j^  ^  n^Jj^)  is  the  effective  inertia  and 

2 

B  =  (B  +  n  B,  )  is  the  effective  damping  coefficient  at  the  actuator 
etT  m  L 

shaft . 

• 

To  express  the  torque  relation  at  the  Load  shaft,  eliminate  0  and  9 

mm 

among  (13),  (14)  and  (18)  to  yield 

T„/n  =  ["(J^  +  J„)/n^  +  J.le’  +  (B„/n^  +  B,  )0^  (19) 

m  l_am  l-Js  m  Ls 

where  KJ  +  J  )/n^  +  J,  1  is  the  effective  inertia  and  (B  /n^  +  B.  )  is  the 
I  a  m  LJ  m  L 


effective  damping  coefficient  at  the  Load  shaft.  equivalent 

generated  torque  at  the  Load  shaft. 

The  actuators  used  in  the  industrial  robots  are  either  hydraulic,  or 
pneumatic,  or  electrical.  As  an  example,  the  Unimation  PUMA  and  the  Stan¬ 
ford  manipulators  have  electrical  system  using  permanent  magnet  dc  motors. 
They  are  armature  controlled  and  their  schematic  diagram  is  shown  in  Figure 
8.  In  this  figure,  v.  <t)  is  the  back  emf  in  volts  in  the  armature  winding 

D 

which  can  be  represented  by 

V.  <t)  =  K.  0„<t)  (20) 

b  b  m 

where  is  the  back  emf  constant  in  voLts-second/radian.  Let  L  and  R  be 
the  inductance  in  Henry  and  resistance  in  ohms  of  the  motor  armature  wind¬ 
ing,  respectively.  Then  by  applying  the  Kirchhoff's  voltage  Law  to  the  ar¬ 
mature  circuit,  one  obtains 

v<t)  -  V.  (t)  =  L  di(t)/dt  +  Ri(t)  (21) 

D 

which  has  an  equivalent  equation  in  frequency  domain  through  a  Laplace 
transformation 

V(s)  -  K.  s  9  (s)  =  (Ls  +  R)I(s)  (22) 

b  m 

where  s  is  the  complex  frequency  in  radians  per  second.  The  dc  motor  is 
operated  in  its  Linear  range  so  that  the  generated  torque  is  proportional  to 
the  armature  current.  The  relation  in  the  frequency  domain  is 

T„(s)  =  K.Ks)  (23) 

m  I 

where  Kj  is  the  torque  constant  in  oz-in/ampere.  The  motor  shaft  is  mechan¬ 
ically  connected  to  an  actuator-gear-load  assembly,  as  indicated  in  Figure 
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8^  with  an  effective  inertia  J  ,,  and  effective  damping  coefficient  B  ,,  at 

eff  eff 

the  actuator  shaft.  The  relation  among  the  mechanical  components  are 
described  by  (18)  which  has  a  Laplace  transform  equivalence 


T  (s)  =  KJ  +  J  +  n^J,)s^  +  (B  +  n^B,  )sl  9  (s) 
m  Lam  L  m  LJm 


=  s'"  +  B  ,,  s)  0  (s)  (24) 

eff  eff  m 

Eliminating  T^(s)  and  I(s)  among  (22),  (23)  and  (24)  yields 

e  (s)  K 

vT55 - 1 - S - - - T 

which  is  the  transfer  function,  or  the  feedforward  gain,  from  the  applied 
voltage  to  the  dc  motor  (input)  to  the  angular  displacement  of  the  motor 
shaft  (output). 

To  construct  a  positional  controller  for  the  angular  displacement  of 
the  load  shaft,  it  is  necessary  to  convert  the  displacement  into  electrical 
voltage  to  actuate  the  dc  motor.  For  a  feedback  (or  closed-loop)  controll¬ 
er,  the  actuating  signal  is  the  error  at  time  t  between  the  desired  and  the 
actual  displacements:  ' 


e(t)  =  0  .(t)  -  9^(t)  (26) 

d  s 

By  means  of  a  potentiometer  or  an  optical  encoder/counter  assembly,  the  dis¬ 
placement  error  is  converted  into  voltage  as 


v(t)  =  K^[e^(t)  -  9^(t)] 


which  has  a  transformed  equivalence 


-li¬ 


ves)  =  KgE(s) 

=  "oC  «d<  =  >  - 

where  K.  is  the  conversion  constant  in  voLts/radian.  Combine  all  the  physi- 

U 

cal  apparatus  together,  one  may  construct  a  positional  controller  whose 
block  diagram  is  shown  in  Figure  9(a).  The  feedforward  gain,  or  the  open- 
loop  transfer  function  is 


0  (s) 
s 

E(s) 


KiKb)_ 


S  LJ  +  (RJ  ,,  +  LB  ,,)S  +  (RB  ,,  + 

eff  eff  eff  eff 


(29) 


which  is  obtained  either  from  the  block  diagram.  Figure  9(a),  or  by  combin¬ 
ing  equations  (25)  and  (28),  and  the  relation  0  (s)  =  n  0  (s) . 

s  m 

Physically  the  inductance  of  the  armature  winding  is  in  the  order  of 
tenth  of  milli-henries  while  its  resistance  is  in  units  of  ohms.  Thus  L  is 
practically  zero  so  that  (29)  may  be  reduced  to 


0,(s)  n 

s  ^  _ 9  I _ 

E(s)  SI:RJ„,,s  +  (RB  ,,  + 

eft  eff  I  b 


(30) 


The  unity  feedback  positional  controller.  Figure  9(a),  then  has  a  closed- 
loop  transfer  function 


0  (s) 

6^ 


0  /E 
s 

1+e  /E 
s 


(31) 


TTT 


eff 


+ 


(RBeff  +  KjK^^)s/(RJ^^^)  +  K3Kj/(RJ^^^) 


It  yields  a  second  order  system  which  is  theoretically  always  stable.  To 
increase  the  response  time,  one  customarily  increases  the  system  gain,  such 
as  increasing  K.,  and  injects  some  damping  into  the  system  to  reinforce  the 


effect  of  back  emf  by  adding  a  negative  feedback  of  the  motor  shaft  veloci¬ 
ty.  This  can  be  done  by  either  using  a  tachometer,  or  computing  the  differ¬ 
ence  in  angular  displacements  of  the  shaft  during  a  fixed  time  interval. 
The  block  diagram  of  the  resulting  controller  is  shown  as  in  Figure  9(b)  in 
which  is  the  tachometer  constant  in  volt-sec/radian,  and  is  the  gain 

of  amplifier  in  volts/volt.  Since  the  feedback  voltage  at  the  motor  arma- 

■  •  • 

ture  circuit  is  now  K.  0  (t)  +  K.K  8  (t)  instead  of  K.  0  (t)  alone,  the  cir- 

bm  Itm  bm 

cuit  equation  (21)  is  modified  as 


v(t)  -  (K.  +  K.K,)0  (t)  =  L  di/dt  +  Ri(t) 
b  Itm 


which  has  a  Laplace  transform 


V(s)  -  (K.  +  K,K,)s  e  (s)  =  (Ls  +  R)I(s)  (33) 

b  It  m 

Thus  the  revised  open-loop  and  closed-loop  transfer  functions  can  be  ob¬ 
tained  simply  by  replacing  K.  by  (K.  +  K,K.)  in  (29)  and  (31),  respectively. 

b  b  It 

Consequently, 


®s(s)  _  nKg  s0^(s) 
lTs5  s  KgE(s) 


nK0Ki 


RJeffS^  +  CRBg^^  +  Kj(Kjj  + 


0  (s)  0  (s)/E<s) 

s  _  s 


For  a  specific  robots  the  numerical  values  of  the  parameters  n,  Kj, 

R/  and  are  either  specified  (by  the  component  manufacturer),  or 
determined  by  experiments.  As  an  example.  Joints  1  and  2  assemblies  of  the 
Stanford  manipulator  contain  respectively  a  U9M4T  and  a  U12M4T  dc  motor  with 
an  integral  tachometer  030/105  by  Photocircuits  Corp.  The  parametric  data 
for  the  motor-tachometer  unit  are  listed  as  follows  C5]: 


Model 

U9M4T 

U12M4T 

Kj  (oz-in/amp.) 

6.1 

14.4 

2 

J  (oz-in-sec  /rad) 
a 

0.008 

0.033 

(oz-in-sec  /rad) 
m 

0.01146 

0.04297 

(volts-sec/rad) 

0.04297 

0.10123 

L  (p-henries) 

100. 

100. 

R  (ohms) 

1.025 

0.91 

(volts-sec/rad) 

0.02149 

0.05062 

f  (oz-in) 
m 

6.0 

6.0 

n 

0.01 

0.01 

The  second  to  the  last  line  is  the  average  friction  torque  f^  exists  in  each 
assembly  which  must  be  overcome.  The  effective  inertia  of  each  joint  of  the 
Stanford-JPL  (Jet  Propulsion  Laboratory)  manipulator  is  listed  as  C6D : 
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Joint  No.  Min.  VaLue/No  Load  Max.  Value/No  Load  Max.  VaLue/FuLl  Load 


1 

1.417  kg-m^ 

6.176  kg-m^ 

9.570 

2 

3.590 

6.950 

10.300 

3 

7.257 

7.257 

9.057 

4 

0.108 

0.123 

0.234 

5 

0.114 

0.114 

0.225 

6 

0.040 

0.040 

0.040 

2 

The  given  values  of  the  effective  inertia  is  in  kg-meter  .  From  Newton's 

second  Law  of  motion^  F  =  ma^  where  mass  m  is  in  kg  and  acceleration  a  is  in 

meters/sec  ;  the  force  F  is  in  kg-meter/sec  which  is  defined  as  the  unit  of 

2  2 

force  "Newton."  The  unit  for  torque  t  is  Newton-meter,  or  kg-meter  /sec  . 

But  T  =  J0  where  the  angular  acceleration  is  in  radians/sec^  so  that  J  is 
.  2  2 

in  Newton-meter-sec  /radian  (or  kg-meter  >.  Now  one  Newton-meter  (or  kg- 
2  2 

m  /sec  )  of  work  is  equivalent  to  0.73756  ft-lb.,  or 

p 

0.73756  X  12  X  16  =  141.61  oz-in.  Consequently  one  kg-meter  (or  Newton- 

2  2 
meter-sec  /  radian)  of  inertia  is  the  same  as  141.61  oz-in-sec  /radian. 

Note  that  the  conversion  constant  and  amplifier  gain  however,  must  be 

determined  from  the  parameters  corresponding  to  the  structural  resonant  fre- 

I 

quency  and  the  damping  ratio  of  the  robot,  which  will  be  discussed  in  the 
following  section. 

As  listed  above,  an  average  friction  torque  f^  of  the  motor-tachometer 
assembly  must  be  overcome  by  the  motor.  Of  course,  the  motor  must  also  com¬ 
pensate  the  Load  torque  and  gravitational  torque  These  quantities 

represent  the  reaction  from  the  physical  burden  to  the  robot.  Schematically 
they  are  inserted  in  the  block  diagram  of  the  positional  controller.  Figure 
9(b),  at  the  point  where  the  torque  is  generated  from  the  motor.  Figure 
9(c)  shows  the  revised  block  diagram  of  the  positional  controller,  in  which 
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F  (s)^  T,  (s)  and  T  (c)  are  the  Laplace  transformed  variables  of  f  ,  t  and 
m  L  g  m  L 

T  .  respectively. 

9 


B.  Determination  of  and  1^^ 

From  (35),  the  closed-loop  transfer  function  can  be  written  as 


e^(s>  ^  nKjKj 


-(36) 


+ 


R0  +  K.(K.  +  K 
eff  I  b 


S/(RJg^^)  +  nKQKj/(RJg^^) 


Thus  the  characteristic  equation  for  the  closed-loop  controller  is 


+  Kj(K^  +  K^K^)Js/(RJ 


eff)  "  "'<6'<i/^'''eff'  =  ° 


(37) 


which  is  conventionally  expressed  as 

+  2Cu  S  +  u>^  =  0  (38) 

n  n 

where  c  is  the  damping  ratio  and  the  undamped  natural  frequency.  From 
(37)  and  (38),  one  obtains 

0)^  =  ^nKgKj/(RJg^^)  >  0  (39) 

and 

^'^“n  "  t®eff 

so  that 

'  =  ['Sff  *  '<l'S  * 

Refer  to  Figure  10  in  which  is  the  effective  stiffness  (in  oz- 

in/rad.)  of  the  joint  of  the  robot.  The  restoring  torque  due  to  stiffness 
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is  Thus,  by  D'Alembert's  principle 

SO  that  the  structural  resonant  frequency  in  radians/sec.  is: 

“r  = 

Although  k^^^  for  the  joint  is  fixed,  varies  as  the  load  varies  so  that 

changes  accordingly.  Let  u  be  the  measured  structural  resonant  frequency 
of  the  same  joint  corresponding  to  the  effective  inertia  J,  then 


Thus,  by  (43)  and  (44), 


“r  =  “  V^'^eff  ^^5) 

The  measured  u  and  its  corresponding  J  for  the  Stanford  manipulator  are 
listed  as  follows  C1D: 


Joint  No. 

J 

f 

a)(=2Trf ) 

1 

5  kg-m2 

4  Hz 

25.1327  rad/sec 

2 

5 

6 

37.6991 

3 

7 

20 

125.6636 

4 

0.1 

15 

94.2477 

5 

0.1 

15 

94.2477 

6 

0.04 

20 

125.6636 

For  a  conservative 

design  with 

a  safety 

factor  of  200%,  one  set 

the  undamped 

natural  frequency 

no  more 

than  one- 

-half  of  the  structural 

resonant  fre- 

quency  [73.  Thus  by  (39)  and  (45),  one  obtains 
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ynKgKj/CRJ^^^)  <  0.  /2  (46) 

which  reduces  to 

K,,  <  (Ja)^)R/(4nK^)  (47) 

O  “  i 

Relation  (47)  establishes  the  upper  bound  of  K  .  It  remains  to  determine 

6 

the  bound  on  .  For  practical  reasons,  one  avoids  the  underdamped  posi¬ 
tional  controller  for  the  robot.  Thus  4^1/  and  from  (41)  one  obtains: 

•^^eff  *  '^I^'^b  '^I'^t^  -  2  V^KgKjRJg^^  >  0  (48) 

Again  for  conservative  design,  K.  at  the  right  side  of  (48)  is  replaced 

O 

2 

by  its  upper  bound  which  is  given  by  (47)  as  (Ju  )R/(4nKj).  Thus  (48) 
reduces  to 

K,  >  R(« 

Since  varies  as  the  load  changes,  the  lower  bound  on  changes  accord¬ 

ingly.  To  simplify  the  design  of  the  controller,  the  amplifier  gain  should 

be  fixed.  Thus  the  maximum  value  of  J  ,,  should  be  used  in  (49)  to  avoid 

ef  f 

any  possibility  of  resulting  in  an  underdamping  system. 

C.  Compensation  for  Steady-State  Errors 

In  the  preceding  section,  the  block  diagram  of  the  positional  controll¬ 
er  for  a  single-joint  robot  was  presented  in  Figure  9(c).  Because  of  an  ad¬ 
dition  of  the  physical  burden  f^,  and  to  the  motor,  the  closed-loop 
transfer  function  of  the  controller  is  not  the  same  as  given  by  (35),  and  it 
must  be  modified  to  include  the  additions.  From  Figure  9(c),  it  is  seen 


that 
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+  B  ,,S)0  =  T  (s)  -  F  (s)  -  T  (s)  -  nT.  (s) 

eTT  eff  mm  m  g  L 


T  (s)  =  K  fvCs)  -  s(K.  +  K,K^)e  (s)/n1/R 
fn  H_  bits  J 


V(s)  =  K^le.Cs)  -  0  (s) 
eL  d  s  _ 


ThuS/  after  some  algebraic  manipulation^ 


0^<s)  =  <nK.K-0.(s)  -  nRlF  (s)  +  T  (s)  +  nT,  (s)l>/n(s)  (53) 

s  Old  Lffl  g  LJ 


where 


tKs)  =  RJg^^s  +  Irb^^^  + 


Kj(K|^  +  K^K^)Js  +  nKgKj 


Whenever  F^(s)^  T  (s),  and  T,  <s)  vanish,  equation  (53)  reduces  to  (35). 
m  g  L 

Since  the  position  error  e(t)  is  defined  as 


e(t)  =.  9  (t)  -  e^(t) 
d  s 


then  (53)  can  be  written  as 


E(s)  =  0  .(s)  -  9  (s) 
d  s 


^^R^eff^  +  [*^®eff  *  '^I'^t^] 

nRlF  (s)  +  T„(s)  +  nT,  (s)l)/n(s) 

L  m  g  L  J 


s>0j(s)  + 


where  E(s)  is  the  Laplace  transform  of  e(t).  For  a  constant  load, 


then  T|^(s)  = 


Since  f  =  C  ,  and  t  =  C  are  also  constant- 
m  f'  g  g  ' 

F^(s)  =  C^/s  and  T^Cs)  =  C^/s.  Consequently  equation  (56)  becomes 


E(s)  =  (CRJg^^s^  +  +  Kj(K^  +  K^K^)Js>X(s) 

+  Cg  +  ncj/s)/fi(s) 


nRlC^  ■*■  ^g  nCj/s)/fi(s) 


where  X(s)  replaces  9j(s)  to  represent  a  generalized  input  command. 

The  steady-state  error  e^^  may  be  determined  by  the  use  of  the  Final 
Value  Theorem  which  states  that 


e  =  lim  e(t)  =  lim  sE(s)  (58) 

ss  _ 

t^“  s-^O 

provided  the  limits  exist. 

(a)  Steady-State  Position  Error  and  Compensation 

If  the  input  is  a  constant  displacement  then 

X(s)  =  9^(s)  =  Cg/s  (59) 

By  (58)^  one  obtains  a  steady-state  position  error 

=  R<C.  +  C  +  nC,  )/(K„K.)  (60) 

ssp  f  g  L  9  I 

Since  Kg  has  an  upper  bound  given  by  (47),.  the  error  may  not  be  reduced  to 
an  arbitrary  small  value  by  merely  adjusting  the  parameter  Kg.  However,  if 
one  knows  the  value  of  x,  ,  f  and  x  in  advance,  it  is  possible  to  feed  for- 
ward  these  quantities  into  the  controller  to  anticipate  the  burden.  Based 
on  this  idea,  an  anticipated  gravitational  torque  signal  x  and  a  desired 

3 

compensating  torque  signal  x  .  are  fed  to  the  controller  as  an  additional  in- 

d 

put,  as  shown  in  Figure  11(a)  where  T  (s)  and  T.(s)  are  Laplace  transforms 

3  0 

of  X  and  X  ,,  respectively.  With  this  arrangement,  the  error  given  by  (56) 
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is  modified  to  become 


E(s)  =  t  U  ♦ 


*  Ktf  ♦  "I'l'b  * 

nR{F  (s)  +  T  (s)  +  nT,  (s)  -  K  K^fr  (s)  +  (s)l /R»/ fi(s) 
m  g  L  IRLa  dj 


where  X(s)  replaces  Q^ls)  to  represent  a  generalized  input  signal.  For  an 
input  of  a  constant  displacement  X(s)  =  ®j(s)  =  Cg/s^  the  steady-state  posi¬ 


tion  error  now  becomes 


Cccr,  =  s-CrFf^^s)  +  T  (s)  +  nT,  (s)l  -  K.K.fT  (s)  +  T  .(s)l>/(K.K.)  (62) 
ssp  (_m  g  Lj  lR|_a  dJ  01 

If  Tg(s)  =  RTg(s)/<KjKj^)  and  T^(s)  =  r[f,^(s)  +  nT|_(s)] /(K^Kj^)^  then  the 
steady-state  error  would  be  zero.  In  practice^  one  may  set: 


^a  =  <«/"lS^’g 


Tj  =  (R/KjKpX?,^  +  n\)  '  <64) 

to  reduce  the  error^  where  f  .  ?  and  f,  are  the  estimates  of  t  ,  f  and  t,  . 

g  Hi  L  in  L 

respectively.  For  a  given  task^  the  value  which  includes  the  compliant 

torque^  is  usually  known.  Thus  can  be  estimated  directly.  The  measured 

value  of  f  through  experimentation  is  usually  used  for  ?  .  The  value  of  f 
m  m  g 

is  normally  computed  which  will  be  discussed  in  the  section  on  Multiple 
Joint  Controller. 
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(b)  Tracking  Error  and  Compensation 

Quite  often  the  robot  is  required  to  follow  a  moving  conveyer  with  a 

constant  speedy  and  perform  a  certain  task  on  the  conveyer.  In  this  case 

the  input  signal  of  the  desired  position  e.Ct)  must  be  updated  very  fre- 

d 

quently,  say  every  1/60  second,  to  synchronize  the  moving  conveyer.  In 

essence  the  input  is  a  ramp  signal  C^t  with  a  constant  slope  so  that 
2 

X(s)  =  C^/S  .  Applying  the  Final  Value  Theorem  to  (61),  one  obtains  the 
steady-state  tracking  error 


sst 


=  [- 


RB  ..  +  K.(K.  +  K 
eff  I  b 


iV]^ 


/(nK.K^)  +  e 
6  I  ssp 


(65) 


Since  the  controller  requires  C  ^  1  to  avoid  underdamping,  (48)  holds  so 
that  (65)  becomes 


sst 


>  4 


(66) 


If  the  controller  is  designed  conservatively  to  require  u  <  u  /2-  then  (47) 

n  —  r 

holds  and  (66)  reduces  to 


®sst  >  V^eff'*^  ^  ®ssp 


(67) 


which  gives  a  lower  bound  for  the  steady-state  tracking  error. 

To  reduce  the  steady-state  tracking  error,  additional  feedforward  sig¬ 
nal  Vj,  corresponding  to  the  desired  constant  slope  of  the  ramp  signal  is 

fed  to  the  controller  at  the  same  place  where  t  and  t  are  injected.  This 

3  G 

is  indicated  in  Figure  11(b)  where  Vj(s)  is  the  Laplace  transform  of  v^.  As 
a  result,  E(s)  given  by  (61)  is  modified  to  become 
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E(s)  =  (CRJg^^s^  +  +  Kj(K^  +  K^K^)Js>X(s)  -  nK^K^^V^Cs) 

+  nR-CF  (s)  +  T  (s)  +  nT,  (s)  -  K.K^fT  (s)  +  T  . (s)] /R» / fi(s) 
m  g  L  IRLa  dj 

so  that  e  ^  in  (65)  now  becomes 
sst 

=  -CIrB  --  +  K.(K.  +  K.K  jlc  -  nK-K„  Lim  sV  .(s)>/ (nK^K^)  +  e 
sst  L  eff  Ib  Itjv  IR  d  9  1  ssp 

Consequently  the  first  term  of  e^^^  vanishes  if 

V  .(s)  =  s(C  /s^)rRB  ,,  +  K.(K.  +  K.K jl/CnK^K^)  (70) 

d  V  Leff  Ib  ItJ  IR 

2 

But  C^/s  =  X(s)  represents  the  ramp  input  signal,  hence 

Vj  =  (dx/dt)rRB^^^  +  Kj(K^  +  K^K^)l/(nKjKj^)  (71) 


(68) 


(69) 


which  can  be  obtained  directly  from  the  input  terminal  of  the  controller  C73 
as  indicated  in  Figure  11(c).  Since  x(t)  is  a  ramp  input  C^t,  or 
X(s)  =  C^/S^,  then  dx/dt  is  the  constant  slope  C^,  or  sX(s)  =  C^/s.  Of 
course  one  may  obtain  dx/dt  by  computing  the  quotient 
[x(t,)  -  x(t,.,)]  /(t^-t^_^),  where  x(t^)  and  x(t^_^)  are  values  of  two  con¬ 
secutive  input  signals.  These  arrangements  will  automatically  take  care  of 
the  steady  state  error  in  the  original  positional  control  mode.  When  the 
controller  leaves  the  tracking  mode  and  enters  the  positional  control  mode, 
x(t)  is  a  step  input  C^,  or  X(s)  =  ©^^(s)  =  Cg/s.  Then  dx/dt  =  Cg6(t),  or 
sX(s)  =  C.  (an  impulse  which  is  absorbed  by  the  energy  storing  elements  of 
the  system),  so  that  the  compensation  for  the  tracking  error  vanishes. 


-  23  - 


(c)  Acceleration  Error  and  Compensation 

To  minimize  the  travelling  time  of  the  robot,  one  often  desires  to  ac¬ 
celerate  it  from  one  desired  velocity  to  the  other  in  a  shortest  possible 
time.  To  do  so,  one  usually  uses  the  constant,  maximum  possible  accelera¬ 
tion.  In  this  mode,  then  x(t)  is  a  parabolic  input  signal  C  t^/2  so  that 
X(s)  =  C  /s^  where  C  is  a  constant.  By  the  Final  Value  Theorem,  (61) 

3  3 

yields  the  steady-state  acceleration  error 


e  =  (CRJ  ,,  + 
ssa  eff 


[' 


RB  ,,  +  K,(K.  + 
eff  I  b 


K,k,>] 


/s>C  -  nK  lim  sV  .(s))/ (nK„K^) 
a  I  R  d  01 

s-*-o 


+  e 


(72) 


ssp 


If  a  feedforward  signal  v^  is  provided  as  indicated  before  and  the  controll 
er  is  connected  as  shown  in  Figure  11(c),  then 


V  .(s)  =  sX(s)Q(s) 
d 

=  (C  /s^)Q(s) 

3 

where 

Q(s)  =  +  Kj(K^j  +  K^K^)]/(nKjK^) 


(73) 


(74) 


Consequently 


ssa  eff  a  0  1  ssp 


(75) 


For  a  conservatively  designed  controller  with  <d^  “p/2,  (47)  holds  so  that 

(75)  becomes 


e  >  4  J  ,,C  /(Jw*-)  +  e 
ssa  —  eff  a  ssp 


(76) 


This  establishes  the  lower  bound  for  the  steady-state  acceleration  error. 


% 
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As  before^  to  reduce  the  error,  additional  feedforward  signal  a  is  required 

d 

to  be  fed  to  the  controller  at  the  same  point  where  is  applied,  as  shown 

in  Kigure  11(d)  with  A.(s)  be  the  Laplace  transform  of  a..  With  this  addi- 

d  d 


tion,  (68)  reduces  to: 


E(s)  =  |RJg|:^s  X(s)  -  nKjKj^A^(s) 


+nR-CF  (s)  T  (s)  +  nT,  (s)  -  K^K^Tt  (s)  +  T  .(s)l/R>l  /n(s) 
m  g  L  IRLa  dJJ 


and  hence 


J  ,,C  -  nK,K  lim  sA_,(s)  /(nK.K,)  +  e 

eff  a  I  R  d  61  ssp 

$■*■0 


Again,  if  one  chooses 

A^(s)  =  s^(Cg/s^)RJg^^/(nKjKj^)  ,  (79) 

the  first  term  of  e  ^  will  be  eliminated.  Since  C^/s^  =  X(s),  the  parabol- 

ssa  a 

ic  input  signal,  A^ls)  may  be  obtained  from  the  input  terminal  directly  C73 
as  shown  in  Figure  11(e),  Since  x(t)  is  a  parabolic  signal  C  t  /2,  then 

O 

2 

dx(t)/dt  =  C  t  is  a  ramp  signal  with  constant  slope,  or  sX(s)  =  C_/s  ;  and 

3  3 

2  2  2 

d  x(t)/dt  =  C  is  a  constant,  or  s  X(s)  =  C_/s.  Again,  the  value  of  dx/dt 
may  be  obtained  by  computing  the  quotient  [j<(t^)  -  x(t^_^)J/(t^  -  t^_^)  and 
that  of  d^x/dt^  by 

(:[x(t.)  -  x(t.  -  t._,,)]/(t.  -  t._^)  - 

r  "1  (80) 

Ix(ti-I)  -  x(t^_^)J/(t._^  -  t ._2)>/(t .-t ._^) 


These  arrangements  also  automatically  handle  the  errors  of  other  control 
modes.  When  the  controller  is  in  positional  control  mode,  x(t)  is  a  step 
signal  and  X(s)  =  Cg/s.  Then  dx/dt  =  €^6(1),  ^  and  sX(s)  =  Cg, 

x/dt  =  a  doublet  or  double  impulse;  ^ 
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2 

s  X(s)  =  sC„  so  that  the  compensations  for  both  the  tracking  error  and  ac- 

U 

ceLeration  error  vanish.  When  the  controller  is  in  tracking  mode,  x(t)  is  a 

ramp  signal  C^t  and  X(s)  =  C^/s^.  Then  dx/dt  is  a  step  input 

2  2  2 
d  x/dt  =C  6(t);  or  sX(s)  =  C  /s,  and  s  X(s)  =  C  which  give  a  correct 

V  V  V 

amount  of  compensation  for  tracking  error,  but  no  compensation  for  accelera¬ 
tion  error  is  provided. 

Also,  in  computing  A.(s),  one  needs  the  value  of  J  This  value  must 

d  efT 

be  estimated  or  approximated  if  it  is  not  available.  To  avoid  an  over  com¬ 
pensation  which  may  result  in  an  overshooting  of  the  output  of  the  position¬ 
al  controller,  one  normally  uses  the  minimum  value  of  C73  since  A^(s) 

is  a  positive  feedforward  signal. 

0.  Multiple  Joint  Controller 

Intuitively,  it  is  not  efficient  to  restrict  the  robot  moving  one  of 
its  joints  at  a  time  alternatively  by  locking  all  the  other  joints.  In  do¬ 
ing  so  the  execution  time  of  a  given  task  is  unnecessarily  long  and  hence 
the  operation  is  not  economical.  But  to  allow  more  than  one  joint  moving 
simultaneously,  force  and  moment  interactions  among  the  moving  joints  result 
which  cause  the  inadequacy  of  the  use  of  the  above  presented  positional  con¬ 
troller  for  each  joint.  Thus  an  additional  compensation  is  needed  to  over¬ 
come  the  interaction.  To  determine  the  compensation  for  interaction,  it  is 
necessary  to  analyze  the  dynamic  behavior  of  the  robot.  { 

(a)  Lagrangian  Formulation  of  Dynamic  Equation 

The  discussion  on  Lagrangian  equation  can  be  found  in  most  of  physics 


textbooks  (e.g.  C83).  It  represents  the  dynamic  behavior  of  a  system  of 

rigid  bodies,  and  it  has  the  form 
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d  ,3L  -  3L  •  ,  , 


where  =  generalized  coordinates. 


L  =  =  Lagrangian 

T.  =  generalized  forcing  function  . 


The  generalized  coordinate  q.  represents  the  position  6^  of  joint  i.  The 
Lagrangian  is  also  defined  as 


L  =  (Kinetic  energy  of  the  system)  -  (Potential  energy  of  the  system). 


By  applying  the  Lagrangian  equation  to  a  robot  with  n  joints  (or  n  links), 
one  obtains  C6,7,93: 


il'k 

where 


'’ii  =  ^  Trfu  .  J,,(l^.)n 

p=max(i,j)  L-P)  -P  -pi  J 


1  _ _ .  p  ^  -pi  -p 


Tr  =  trace  operator. 
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=  transpose  of  (  ), 

=  input  generalized  force  for  joint  i, 

=  mass  of  Link  p, 

=  a  vector  describing  the  center  of  mass  of  Link  p  with 
respect  to  p-th  coordinate  system^ 

=  CO^  0,  9.8  m/sec2,  03  is  a  gravitational  acceleration 
vector  at  a  sea  level  base^ 

=  inertia  matrix  for  Link  p. 


U  .  = 
-P] 


ij  -for  P  > 

0  ,  otherwise. 


U  ,  =  - —  = 

-P]k 


for  p  ^  k  ^  j. 


forp>j>.k 


otherwise. 


,  if  joint  j  is  rotational. 


,  if  joint  j  is  translational. 


=  4  X  4  matrix  which  transforms  any  vector  expressed  in 
k-th  coordinate  system  to  the  same  vector  expressed 


in  j-th  coordinate  system. 


=  generalized  coordinate  (i.e.^  joint  displacement). 

(b)  Coupling  Between  Joints  and  Compensation 

For  each  joint  i^  the  required  torque  or  force  is  divided  into  five 

groups  as  shown  in  (82).  The  first  group  represents  the  contribution  from 

inertias  of  all  the  joints.  Unlike  the  single-joint  case  in  which  all  other 

joints  are  locked  and  inertias  of  all  joints  are  lumped  together^  now  there 

are  contributions  from  coupling  inertias  between  joints.  These  torque  terms 
n 

Z  0..  q.  must  be  fed  forward  in  the  controller  for  joint  i,  as  shown  in 

j=1  J 
j^i 

Figure  12,  to  compensate  the  interaction  between  joints.  The  second  term  in 
(82)  represents  the  inertia  torque  of  the  actuator  of  joint  i  which  has  al¬ 
ready  been  included  in  J^^^-term  as  outlined  during  the  discussion  on  the 
single-joint  controller.  The  last  term  is  resulted  from  the  gravitational 
acceleration,  which  has  also  been  compensated  by  the  feedforward  term  t  . 

d 

This  is  the  anticipated  gravitational  torque  signal  which  must  be  computed 

by  (63),  i.e.,  t  =  (R/K-Ko)/t  where  t  is  the  estimate  of  gravitational 
a  1  K  g  g 

torque  Xg.  Intuitively,  one  uses  D^.  for  the  best  estimate  for  for  joint 
i  controller.  Thus  by  (85),  one  sets 


=  D.,  =  -  z 


(89) 


p=i 


for  joint  i. 

The  third  and  fourth  groups  in  (82)  represent  the  contributions  from, 
respectively,  the  centrifugal  and  the  Coriolis  forces.  Again  these  torque 
terms  must  be  fed  forward  in  the  controller  for  joint  i  to  compensate  the 
physical  interactions  between  joints  as  shown  in  Figure  12.  This  figure 
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depicts  the  complete  block  diagram  of  the  controller  for  joint  i  of  an  in¬ 
dustrial  robots  i=1^2^***^n.  To  implement  these  n  controllers^  the  values 
of  the  feedforward  elements  must  be  computed  for  the 
specific  robot,  which  will  be  discussed  in  the  following  sections. 

(c)  Computation  of  Compensation  for  Coupling  Inertia 

The  computation  of  terms  is,  unfortunately,  very  complicated  and 
time  consuming.  To  illustrate  the  difficulty,  equation  (82)  is  expanded  for 
a  six-joint  robot,  n=6,  as  follows: 


“n"!  *  “i2‘>2  *  •••  *  *  ■'ai-’i 


*  *  “i22^2  *  •••  *  “i66\ 


*  “n2'<l'’2  *  *  •"  * 


*  ‘‘U5’4'<5  *  •••  * 


i56'^5''6  ‘'i- 


For  i=1,  the  term  is  further  expended  as  shown  in  Figure  13  in 
which  9.  =  q^,i=1,2,***,6.  Obviously  it  is  not  a  simple  computational  task 
especially  when  the  position-dependent  and  orientation-dependent  parameters 
change  as  the  robot  moves.  Therefore  it  warrants  the  effort  of  search  for 
methods  of  simplifying  the  computation.  There  are  three  known  approaches  of 


simplification  CIO, 11, 73,  viz.  geometric/numeric,  composite,  and  differen¬ 
tial  transformation.  The  geometric/numeric  evaluation  deals  with  the  nature 


of  joints  whether  it  is  revolute  or  prismatic.  Thus  the  T_j  matrices  in 
(86),  (87)  and  (88)  can  be  simplified  in  advance.  Since  many  elements  in 
the  A  by  A  matrices  are  zeros,  the  resulting  expressions  for  D^.,  D.^  and 
D...  are  less  complicated  C10D.  The  composite  technique  C11]  involves  the 

1  3  K 

comparison  of  all  the  terms  in  Newton-Euler  formulation  of  the  dynamic  equa- 
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tion  l12]  in  a  computer.  Some  of  the  terms  may  be  eliminated  under  various 
criteria.  The  remaining  terms  are  then  rearranged  in  a  Lagrangian  formula¬ 
tion.  The  upshot  is  a  computer  output  of  a  simplified  equation  in  symbolic 
form.  The  differential  transformation  C7D  which  converts  the  partial 
derivatives  of  the  matrix  transformation  3T^/3q^.  into  the  matrix  product  of 
the  transformation  and  a  differential  matrix  which  reduces  to  a  much  simpler 
form.  The  third  approach  will  be  discussed  in  the  following.  To  facilitate 
the  discussion^  it  is  necessary  to  introduce  the  homogeneous  transformation 
which  is  a  4  by  4  matrix  that  represents  the  rotation  and  translation  of 
vectors  in  some  coordinate  systems. 


Refer  to  Figure  14<a)  which  shows  two  aligned  coordinate  systems 

III  III 

<_X/X/1.^  and  ^  ^  point  P  is  fastened  upon  (x^  i.e.,  when 

I  I  I 

<£  ,z_  )  moves  with  respect  to  point  P  moves  with  it.  Supposing 

I  I  I 

<£  /£  )  is  rotated  y  radians  about  z-axis  as  shown  in  Figure  14(b). 

I  I  I 

Since  P  moves  together  with  <£  its  location  in  that  coordinates 

remains  unchanged.  However^  the  location  of  P  in  (x.^^^£.)  coordinates 
changes.  Refer  to  Figure  14(c)^ 


But 


a^  =  OP  cos  C9+y)  =  OPCcos0  cosy  -  sin6  sinY^ 
b-j  =  OP  sin  (9+y)  =  IJPCsin9  cosy  +  cos9  sinY] 


a  =  OP  cos  0 


(91) 


b  =  Up  sin  9 


(92) 
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=  a  cos  Y  "  b  sin  y 
=  a  sin  y  +  b  cos  y 
c^  =  c 

which  can  be  written  as 


S-i' 

"a" 

1 

"cos  Y  -sin  Y  0  (f 

sin  Y  cos  Y  0  0 

b 

0  0  10 

c 

1 

1 

o 

o 

o 

1 

^  * 

(93) 


(94) 


The  reason  for  the  addition  of  the  fourth  row  and  fourth  column  in  the  ma¬ 
trix  of  the  above  equation  will  be  clear  when  the  translation  of  the  posi¬ 
tion  is  introduced.  In  the  mean  time  the  matrix  in  (94)  i^  the  homogeneous 
transformation  which  rotates  the  vector  or  point  P,  and  hence  the  coordi- 

I  I  I 

nates  (£  ,  y  radians  about  z-axis.  For  convenience,  the  matrix  is 

denoted  by  ^(2,y)  so  that  (94)  may  be  written  as 


"a" 

=  R.<£/Y) 

b 

c 

1 

1 

Likewise, 


"1 

0 

0 

Cf 

"cos  0 

0 

sin 

0  0" 

0 

cos 

a 

-sin  o 

0 

0 

1 

0 

0 

R^(Xy 

o)  = 

0 

sin 

a 

cos  a 

0 

/ 

R^(21,B)  = 

-sin  0 

0 

cos 

0  0 

(96) 

0 

0 

0 

1 

0 

0 

0 

1 

Now 

supposing 

1 

(£ 

1 

1 

,z_  )  is 

rotated 

S  radians 

about  y 

-axis. 

Again,  the 

lo- 
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I  I  I 

cation  of  point  P  in  (x^  ^21  J  does  not  change  but  in  changes  from 

(a^^b^/C^)  to  as 


"2 

V 

b 

*=2 

=  R(y,B) 

=  ^(y^B)^(£^Y) 

c 

1 

1 

1 

^  * 

or 


(97) 


"  COSB  COSY 

-cosB  siny 

sinB 

cr 

siny 

COSY 

0 

0 

-sinB  COSY 

sinB  siny 

cosB 

0 

0 

0 

0 

1 

(98) 


Thus^ 


(97)  and  (98)  show  that  coordinates 


I  I  I 

(x_  ^21  ^  is  first  rotated  y 


ra- 


dians  about  z-axis^  and  then  rotated  B  radians  about  y-axis.  If  it  is 


further  rotated  o  radians  about  x-axis,  then  one  will  have 


■^3' 

~a 

^3 

^2 

b 

*^3 

=  ^(x^,a) 

*=2 

c 

1 

1 

1 

w  m 

(99) 


or 
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11 

'^12 

^^13 

21 

R22 

*^23 

31 

'^32 

'^33 

0 

0 

0 

ri 

V 

b 

c 

1 

where 


=  COSB  COSY 
R^2  “  “COSB  siriY 
R^j  =  sinB 

R2^  =  cosa  sinY  +  sina  sinB  cosy 

R22  =  cosa  COSY  "  sina  sinB  sinY 

R23  =  -sina  cosB 

Rj^  =  sina  sinY  -  cosa  sinB  cosy 

R32  =  sina  cosy  +  cosa  sine  sinY 

R33  =  cosa  cosB 


Iff 

Now  supposing  ^ x.  )  is  translated  t^,  t^  and  t^  unitS/ 

along  x-axis^  y-axis^  and  z-axis.  Then 


"3' 

t  ■ 
X 

Si 

S2 

*^13 

t  " 
X 

V 

\ 

^>3 

t 

y 

Si 

S2 

•^23 

t 

y 

b 

‘=4 

*^3 

+ 

t 

z 

Si 

S2 

*^33 

t 

z 

c 

1 

1 

0 

0 

0 

0 

1 

1 

. 

-- 

(100) 


(101) 


respectively. 


(102) 


Let 
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L(t  ) 

—  X  y  z 


1 

0 

0 

0 


0  0 
1  0 
0  1 
0  0 


t 

X 

t 

y 

t 

z 

1 


(103) 


denotes  the  above  mentioned  Linear  translation^  then  the  4  by  4  matrix  in 
(102)  may  be  written  as 


«11 

S2 

S3 

t  ■ 
X 

«21 

S2 

S3 

t 

y 

L<t„,t„,t,)R(x,a)R(y,B)R(z,Y)  = 

A  7  £ 

Si 

S2 

S3 

t 

z 

0 

0 

0 

1 

a 

(104) 


which  represents  a  rotation  of  y  radians  about  z-axis/  then  a  rotation  of  S 

radians  about  y-axis  followed  by  a  rotation  of  a  radians  about  x-axiS/  and 

finally  a  translation  of  t  ^  t  ^  t  ^  respectively  along  the  x,  y,  and  z- 

X  y  z 

axes.  Thus  the  4  by  4  matrix  which  is  called  the  homogeneous  transforma¬ 
tion^  includes  rotation  as  well  as  translation  of  the  coordinates 

I  I  I 

(j^  J  •  Since  the  matrix  multiplications  do  not  commute,  the  order  of 

multiplying  the  matrices  ^  and  ^'s  in  (104)  cannot  be  interchanged.  Should, 

for  example,  ^(x^a)  and  IR(2_/B),  or  L.(t  ,t  ,t  )  and  R(z^y)  interchange  their 

X  y  z 

places  in  (104),  the  resulting  matrix,  and  hence  the  physical  order  of  rota¬ 
tions  and  translation  would  be  different. 

Refer  to  Figure  15  and  suppose  that  the  p-th  joint  of  the  robot  is  ori¬ 
ginally  at  the  point  P.  Then 
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jP  _  — p  — p  — p 

— o  0  o  o  1 


(105) 


represents  the  coordinate  frame  with  respect  to  the  base  coordinates.  Now 

rotate  the  joint  in  the  following  order:  y  radians  about  z  -axis.  S  radians 

o 

about  y^-axis,  a  radians  about  x^-axis;  and  finally  translate  ^  with  respect 
to  coordinates.  Supposing  the  resulting  orientation  and  position 

of  the  joint  is,  respectively,  and  with  reference  to 

(x  ,y  ,z  ).  Thus  the  current  state  of  the  hand  with  reference  to  base  coor- 
dinates  can  be  represented  by: 
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Now  if  is  perturbed  by  a  small  translation  and  rotation  with  respect 
to  the  base  coordinates  then  t  ♦fix,  t  ♦fiy,  t  ♦fiz,  a^6a, 

Y  n  ^  w  Q*  ^  ^  r\  ^  r\^  r\  ^ 
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On  the  other  hand,  one  may  express 
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Combining  (106),  (107)  and  (108)  yields  the  perturbation 
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The  matrix  in  (109)  is  the  variational  operator  with  respect  to  the  base 
coordinates.  If  the  variation  is  referred  to  p-th  joint's  own  coordinates, 
then  it  must  be  premultiplied  by  a  4  by  4  transformation  matrix 

T°  =  (T^)"*'  (110) 
-P  -o 

which  transforms  any  vectors  or  coordinate  frames  with  reference  to  base 
coordinates  J  to  the  p-th  joint  coordinates  (x  ,y  ,z  ).  Thus  the 
perturbation  on  the  p-th  joint  coordinate  frame  with  reference  to  its  own 
coordinates  is 


«  T^  =  (T^)"*' 
p-o  — o 
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Since 
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6  = 
p-o 


where 
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)/ 


and  X  =  cross  product. 
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By  the  vector  identities 
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reduces  to 
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Since  6  is  the  perturbation  on  the  p-th  joint  coordinate  frame  with 

p-^ 

reference  to  its  own  coordinates^  then  based  on  the  results  in  (109),  it  can 


be  written  as 
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where  T  is  an  identity  matrix,  so  that 


6  01  =  X  A 

P  -T>-P 

I 

6_B  =  y_A 


V  =  Vp 

• 

6  Y  =  2  A 
P  -P-P 

,  (116) 
5  X  =  X  (A  Xi  +  d  ) 

P 

fi_y  =  5(1  +  d 

P  -p  — p 

«  2  =  2*  (A  XA  +  d  ) 
p  — p 

Equations  (115)  and  (116)  serve  as  the  basis  for  numerical  computation  be¬ 
cause  of  their  reduced  form.  Now,  if  the  perturbation  on  the  p-th  joint 
coordinate  frame  is  expressed  with  reference  •  to  j-th  joint  coordinates 


(x.j,2^j,£j)  then  it  has  a  form 


A.jP  =  tP(«  T^) 
]_o  -]  p-o 


(117) 


As  a  limit. 


^  dq.  =  T?(6  tP) 
)  —)  P^ 


(118) 


Through  a  lengthy  algebraic  manipulation,  reference  7  shows  that  (with  the 
condition  that  all  the  cross  inertia  terms  are  ignored  since  they  are  rela¬ 
tively  insignificant  by  experiments  C6,73): 


D..  =  2^  tn  -c  (6  o.)k  (6  a.)  +  (6  B.)k  (6  0.) 

p=ma7(i,j)  P  L  P  T  Pxx  p  3  P  i  pyy  P  3 


(6  Y.)k^  (6  Y.)  +  fd^d  )'(T?d  )1 

p'l  P22  p'3  L— i-P  -3-P  J 

[?'((T?d  )X(T?a  )  +  (T?d  )X(t‘?A  ))  > 
P  -i-p  -3-P  -3-P  -  -P 


(119) 


where  fi  a.  is  a  smaLL  rotation  of  coordinates  frame  of  3’oint  p  about  x-axis 
P  T 

with  respect  to  i-th  joint's  coordinates,  and  k^^^  is  the  radius  of  gyration 
"xx"  of  joint  (or  Link)  p  about  the  origin  of  p-th  joint's  coordinates,  ••• 


(d)  Computation  of  Compensation  for  Gravity,  Centrifugal  and  Coriolis 
Terms. 

From  (85)  and  (86)  one  obtains 


"  53  I"-  1  -g?  lo 
p=i  P  ^‘’i  “P 


By  some  algebraic  manipulation,  reference  7  also  shows  that 


D.  =  r  D  m^  (r'^ 
'  “  p=i  P  -P 


(120) 


(121) 


where  =  a  vector  describing  the  center  of  mass  of  Link  p  with  respect 

to  (i-l)th  coordinates,  and 


^  joint  p  is  revolute, 

L  =  • 

1  CO  0  0  ”§  if  joint  p  is  prismatic. 


Since  the  term  D..  contains  a  second  partial  derivative  3  T^/Oq.3q,  ), 
IjK  —0  3k 

it  is  not  able  to  simplify  (84)  for  computation.  Conventionally,  one  often 
ignores  the  centrifugal  and  Coriolis  terms.  The  justification  is  that  these 
two  terms  are  velocity-dependent.  When  the  robot  starts  to  move  from  its 
initial  location  and  approaches  its  goal  location,  the  velocities  are  usual¬ 
ly  low  and  hence  the  contributions  from  these  two  terms  are  insignificant. 
Once  it  picks  up  the  velocity,  the  robot  is  travelling  in  the  space  and  nor¬ 
mally  the  travelled  path  is  not  of  importance.  Should  the  path  is  impor¬ 
tant,  such  as  avoiding  collision  with  obstacles,  then  these  two  terms  may 
not  be  ignored.  They  must  be  computed  either  by  equation  (84),  or  using 
Newton-Euler  formulation  approach  C123  which  is  a  computational  scheme. 
This  scheme  has  been  proven  to  be  computationally  efficient  C133.  Also, 
Bejczy  C6,10]  used  geometric/numeric  approach  to  show  that  for  the  last  four 
joints  of  the  Stanford-JPL  manipulator,  which  has  six  joints  (n=6),  the  fol¬ 
lowing  terms  are  identically  zero; 
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Thus  it  is  possible  to  reduce  the  computational  task  if  the  geometrical  con¬ 
figuration  of  the  robot  is  known  and  if  an  analysis  is  carried  out. 
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IV.  PATH  TRACKING 

Refer  to  the  questions  raised  at  the  end  of  Section  III.  If  the  robot 
is  required  to  travel  along  a  prescribed  path,  the  controller  must  keep  up 
with  path  tracking.  With  positional  controllers  the  path  tracking  can  be 
accomplished  by  dividing  the  Cartesian  path  into  a  number  of  segments.  Each 
end  point  of  the  segments  is  transformed  into  joint  coordinates,  and  then 
the  positional  control  is  applied  from  point  to  point  in  joint  coordinates. 
This  approach  was  briefly  mentioned  previously  in  Section  Il-C(b)  on  track¬ 
ing  errors.  A  number  of  facts  related  to  this  approach  should  be  mentioned. 
By  transforming  all  the  end  points  of  segments  of  Cartesian  path,  one  essen¬ 
tially  constructs  n  corresponding  paths  in  joint  coordinates,  one  for  each 
of  the  n  joints.  If  these  segments,  |d£(t)  d^(t)*J,  are  very  short,  the 
increments  of  joint  displacement,  dq.,  between  adjacent  points  are  very 
small  so  that  sin  dq.  »  dq.  and  cos  dq.  »  1.  Thus  the  transformation  f(*) 
defined  by  (5)  becomes  a  differential  transformation  which  is  usually 
linear.  This  transformation  is  the  Jacobian  matrix  of  the  displacement, 
which  contains  trigonometric  functions  of  the  joint  displacement  with 
respect  to  the  joint  coordinates  before  the  differential  increment  takes 
place  C143.  Analytically,  the  solution  dq.  in  terms  of  d£  and  d£  can  be  ob¬ 
tained  simply  by  inventing  the  Jacobian  matrix.  Although  it  is  sometimes 
possible,  it  is  usually  difficult  since  the  Jacobian  is  quite  complicated. 
Numerical  solution  is  also  possible  but  usually  requires  long  computing 
time.  Moreover,  the  Jacobian  matrix  becomes  singular  when  the  robot  reaches 
a  degenerate  position  at  which  the  solution  dq^  is  not  unique  (i.e.,  more 
than  one  value  of  dq.  yields  a  same  d£  and  de^.)  An  alternative  method  proven 
to  work  successfully  is  to  differentiate  the  solution  of  equation  (5) 
directly  C143.  This  is  possible  since  for  a  given  robot  with  fixed  dimen- 


-1 

sions,  the  transformation  ^  is  known.  Using  this  approach^  one  must  set 
dq.  to  zero  if  it  is  physically  impossible  due  to  constraints,  or  if  it  is 
undetermined.  It  usually  results  in  a  simpler  expression  C14]. 

The  desired  Cartesian  corner  points  of  the  path  may  be  fed  into  the 
control  system  either  numerically,  or  through  the  teaching  by  doing  pro¬ 
cedure.  The  intermediate  points  of  the  small  segments  of  the  path  are  usu¬ 
ally  specified,  or  interpolated  by  the  computer-controller,  or  sampled  and 
recorded  through  teaching  by  doing. 

By  controlling  the  robot  from  point  to  point  using  the  positional  con¬ 
troller,  it  has  a  natural  tendency  to  stop  at  each  point.  This  undesirable 
phenomenon  may  be  eliminated  by  specifying  a  nonzero  velocity  at  each  Carte¬ 
sian  point,  which  can  be  transformed  into  the  corresponding  nonzero  joint 
velocities  by  a  Jacobian  matrix  of  the  velocity.  The  joint  velocities  are 
often  referred  to  as  the  "resolved  rate"  C153.  For  implementation  a  veloci¬ 
ty  control  loop  must  be  added  to  each  controller.  Alternatively,  one  may 
take  advantage  of  the  associated  digital  computer  to  compute  the  required 
joint  torques  which  would  yield  appropriate  velocities  at  fixed  accelera¬ 
tions.  The  computation  requires  the  knowledge  of  the  dynamical  equation  of 
the  robot.  Since  the  robot  is  a  nonlinear  mechanism  with  couplings  among 
its  joints,  as  shown  previously  in  Section  III-D(a)  on  Lagrangian  formula¬ 
tion,  the  computation  is  time  consuming. 

A  possible  way  to  avoid  the  storage  of  numerous  pre-computed  points  of 
joints  paths  or  the  computation  of  these  points  on-line  is  to  determine  the 
functional  representation  of  n  joint  paths.  However,  the  transformation 
in  (5)  is  pointwise.  Since  no  transformation  of  a  function  is  known,  the 
curve  fitting  procedure  may  be  adopted  as  follows  C16,17,183.  The  corner 
points  of  the  Cartesian  path  are  first  transformed  into  joint  coordinates. 
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A  cubic  function  is  assigned  to  each  segment  between  every  two  adjacent 
joint  coordinate  points.  The  adjacent  cubic  functions  are  then  spLined  to¬ 
gether  with  the  continuity  conditions  of  desired  velocity  and,  if  required, 
desired  acceleration.  Curve  fitting  is  done  by  using  a  large  number  of 
points  on  the  path,  after  they  are  transformed  into  joint  coordinates,  and 
applying  the  least  square  error  fit  to  obtain  the  coefficients  of  the  spline 
function.  Thus  one  needs  to  store  only  the  coefficients.  The  points  on  the 
joint  paths  can  be  obtained  by  a  simple  computation  when  needed. 

V.  CONCLUSION 

For  industrial  robots  whose  assignments  are  limited  to  usual  tasks  in¬ 
volving  mainly  synchronization,  their  controllers  can  be  designed  by  conven¬ 
tional  methods  based  on  two  factors:  damping  factor  and  structural  resonant 
frequency.  It  is  possible  to  reduce  the  steady-state  position  error  and  to 
eliminate  the  tracking  and  acceleration  errors  by  feedforward  compensations. 
For  multiple  joint  controllers,  further  feedforward  compensation  is  neces¬ 
sary  to  encounter  the  force  interactions  between  joints.  However,  it  intro¬ 
duces  the  computational  difficulties  so  that  methods  of  approximation  or 
simplification  are  needed. 
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WHAT  DID  WE 
NEGuaTonu 
YOU  ABOUT 

THE  TOMORROW  TOOL? 

There  are  plenty  of  facts 
we  left  out.  And  there  are 
plenty  of  details  about  the 
case  histories*  presented. 
Then  there  are  the  ques¬ 
tions  in  your  mind  tiiat  we 
never  even  thought  of. 

We'd  like  to  answer  them 
for  you.  We'd  like  to  show 
you  how  The  Tomorrow  Tool 
might  make  your  plant  more 
productive,  more  profitable. 
And  even  safer.  Phone  or 
write  us  with  any  question. 
Industrial  Robot  Division, 
Cincinnati  Milacron, 

4701  Marburg  Avenue. 
Cincinnati.  Ohio  45209. 
Phone  513/841-8386. 


CL»0W 

tXTENSiON 


THE  TOMORROW  TOOL 
DOES  WORK  PEOPLE 
CAN’T  DO. 


BASIC  SPECS  FOR  THE  TOMORROW  TOOL. 

Floor  space  and  net  weight 

T^  Industrial  robot  . .  .9  sq.  ft.  (0.8  sqm)  5,000  lb  (2267  kg) 
Hydraulic  power  supply.  17  sq.  ft.  (1.5  sqm)  1,200  lb  (544  kg) 
Electrical  power  unit ...  3.4  sq.  ft.  (0.3  sqm)  700  lb  (317  kg) 
ACRAMATIC  computer 

control . 10  sq.  ft.  (0.9  sqm)  1.100  lb  (498  kg) 

Load  Capacity 

Load  10"  (254  mm)  from  tool  mounting  plate.  120  lbs  (54  kg) 
Load  at  tool  mounting  plate,  max.  velocity  ..  1751bs(79kg) 
Load  at  tool  mounting  plate,  reduced  velocity,  depends  on 

arm  and  wrist  attitude* . 300  lbs  ( 136  kg) 

Positioning  accuracy,  axis  drive 

Accuracy  to  any  programmed  point .  ±  0.050-in.  (±  1.27mm) 

Drive  for  each  of  6  axes . Direct,  electrohydraulic 

Jointed-arm  motions,  range,  velocity 

Max.  horizontal  sweep .  .  .  .  240° 

Max.  velocity  of  tool  center  point . 50  ips  (1270  mmps) 

Pitch . 190P 

Roll . . 240P 

Yaw .  .  .  180P 

Power  requirements .  .  230/460  volts.  3  phase.  60  Hz.  22  KVA 
Environmental  temperature* . 40  to  120P  F  (5  to  50P  C) 

Minicomputer  memory  capacity . 400  points  std. 

(additional  storage  optionally  available) 

Isciory  to*  SOWC'Sl  •po'*<atio*^» 
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Unimatioli  Inc.  PUMA  Robot 
Specifications 


600  Series 


General 

Configuration 
Drive 
Controller 
Teaching  method 
Program  language 
Program  capacity 
External  program  storage 
Gripper  control 
Optional  accessories 


6  revolute  axes 
Electric  DC  servos 
System  computer 

By  teach  pendant  and/or  computer  terminal 
VALTM 

As  required  by  application  (4K  RAM  user  memory  std.) 
Floppy-disk  (optional) 

Computer-controlled,  pneumatic,  0.5  cfm  @  100  psi. 

CRT  or  TTY,  floppy-disk,  pneumatic  grippers  (w/o  fingers) 
I/O  module  (115  VAC  compatible),  continuous  path 


'  Power  requirement 

95-130  Vac,  50-60  Hz.  750  VA  max. 

Performance 

Physical  characteristics 

I 

Repeatability 

:i:0.004  in.  (.1mm) 

Arm  weight 

120  lbs. 

Load  capacity 

5.0  lbs.  (including  gripper)  (2.27KG) 

Controller  size 

16.5  in.  X  19.0  in.  x  20.0  in. 

* 

Tip  velocity 

3.3  ft./sec.  max.  with  max.  load 

(H  X  W  X  D) 

Static  force  at  tip 

13.2  lbs.  max. 

Controller  weight 

75  lbs. 

m  , 

Controller  cable  length 

15  ft.  max. 

i 

WAIST  ROTATION  320’ 

/ 

SHOULDER  ROTATION  JOO’ 


ELBOW  ROTATION  270’ 
'WO  in.  ^ 


WRIST  BEND  200* 


FLANGE 
ROTATION  270* 


GRIPPER  MOUNTING 


WRIST  ROTATION  300* 


Unimation  Inc.  (L 

A  CO^O^C  C.mpvrsf 

Shelter  Rock  Lane,  Danbury.  Connecticut  06810 

Untmtiion^  •  r«oistereO  lfu0*fn«rk  of  Unimation  Inc 
(203)  744-1800 
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Figure  9.  Block  Diagram  of  a  Positional  Controller. 


Figure  12.  Block  Diagram  of  a  Complete  Controller  for  Joint  i  of  a  Robot 
Having  n  Joints. 
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Figure  13.  Coefficient  of  Inertia  Term  for  Joint  1 
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Differential  Kinematic  Control  Equations  for  Simple 
Manipulators 

RICHARD  P.  PAUL,  senior  member,  ieee,  BRUCE  SHIMANO, 
AND  GORDON  E.  MAYER 


Absiraci— The  Jacobian  representing  the  ditferential  change  in  position 
and  orientation  of  the  manipulator  end-e(fector  in  terras  o(  differential 
changes  in  joint  coordinates  Hill  be  presented  together  with  the  inverse 
Jacobian. 


Introduction 

A  general  method  for  obtaining  the  kinematic  equations  for 
any  manipulator  has  been  developed  and  demonstrated  for  the 
PUMA  manipulator  [2].  The  kinematic  equations  have  the  homo¬ 
geneous  transformation  representing  the  position  and  orientation 
of  the  end-effector  as  the  dependent  variable  and  the  joint 
coordinates  as  the  independent  variables.  In  the  case  of  simple 
manipulators,  such  as  the  PUMA  arm.  it  is  possible  to  solve  these 
equations  to  obf;iin  the  joint  coordinates  given  the  position  and 
orientation  of  the  end-effector  [2], 

In  this  correspondence  we  develop  a  general  method  for  ob¬ 
taining  the  Jacobian  with  differential  change  in  position  and 
orientation  as  dependent  variables  and  differentia  change  in 
joint  coordinates  as  independent  variables.  A  method  of  obtain¬ 
ing  the  Jacobian  was  developed  by  Uicker  in  terms  of  the 
differential  change  of  transform  elements  (4],  Groome  (IJ  devel¬ 
oped  the  Jacobian  in  terms  of  a  differential  translation  and 
rotation,  based  on  vector  methods.  Whitney  and  Klumpp  [3] 
adapted  this  method  to  the  standard  form  of  assigning  manipula¬ 
tion  coordinates.  Their  results  wefe  obtained  in  a  coordinate 
frame  located  at  the  manipulator’s  end-effector  but  aligned  with 
the  base  coordinate  frame.  A  further  matrix  multiplication  was 
required  to  obtain  the  manipulator  Jacobian.  In  this  correspon¬ 
dence  we  combine  the  matrix  methods  of  Uicker  with  the  vector 
representation  of  Groome  to  provide  a  straightforward  algorithm 
to  obtain  the  Jacobian  directly  from  the  kinematic  equations.  The 
method  leads  almost  directly  to  equations  resulting  in  the  mini¬ 
mum  number  of  arithmetic  operations. 

If  the  manipulator  has  a  solution  obtained  by  the  methods 
developed  in  [2],  then  the  Inverse  Jacobian  can  be  obtained 
directly  by  differentiating  the  solution.  This  task  is  enormously 
simplified  by  performing  this  differentiation  in  terms  of  the 
change  in  the  transformation  elements  and  in  terms  of  the 
differential  change  of  each  joint  coordinate  as  it  is  obtained.  This 
results  in  very  simple  expressions  for  the  differential  change.  The 
equations  representing  the  kinematic  equations  and  their  solution 
for  the  PUMA  arm  are  included  in  Appendix  I  for  reference.  It  is 
assumed  that  the  reader  is  aware  of  their  derivation  and  signifi¬ 
cance,  if  not,  it  is  suggested  that  (2]  be  reviewed. 

The  jACOBtAN 

The  differential  change  dT,  of  any  transformation  T  can  be 
expressed  in  terms  of  the  differential  change  of  its  elements  or  in 
terms  of  a  differential  translation  d^i  +  d^j  +  d^k  and  a  dif- 
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ferential  rotation  S^i  •+  S^j  •+  6,*  with  respect  to  T.  The  relation¬ 
ship  between  the  two  forms  is  given  by 


where 


and 


Thus 
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In  the  case  of  a  manipulator,  describes  the  end  of  the  manipu¬ 
lator.  We  will  evaluate  Aj-,  as  a  function  of  changes  of  joint 
coordinates,  and  if  dT,,  is  desired  then  we  simply  prcmultiply 
by  7^  to  obtain  dT^.  If  in  the  case  of  a  mampulator  we  were  to 
make  a  change  with  respect  to  a  link  coordinate  frame  ji  —  1  of 
A,  we  could  find  an  equivalent  change  in  T^,  Aj-^  expressed  as 

T,*  Aj.,  =  A,  •  •  ■  •  ■  •  A„^^•  A„»  ••••A,  (5) 

or  on  simplifying 

^76=  . •••  *-<6)-  (6) 


If  link  n  follows  a  revolute  joint  then  a  change  of  joint 
coordinate  d6„  corresponds  to  a  rotation  about  the  z  axis  of  the 
link  n  —  1  coordinate  frame  or 


0  -dff  0  0 

d9  0  0  0 

0  0  0  0 

.  0  0  0  0. 


(7) 


If  the  link  follows  a  prismatic  joint  then  the  change  of  joint 
coordinate  dd„,  corresponds  to  a  translation  along  the  z  axis  of 
lhclink„_,  coordinate  frame; 


pnsmauc 


0  0  0  0 

0  0  0  0 

0  0  0  dd 

0  0  0  0  . 


(8) 


If  we  define  U„as  U„  =  {A„*  /4„  +  i  • 


U  = 


Ox 


0  0  0 


•  Aj")  with  elements 


P. 

Py 

p.- 
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(9) 


Then 

A  (10) 

and 


^76  = 


0 

a,ny~  Oytt, 

a  ,0  y  a  y  fi , 

Px  ^  y  Py  ^  K 

n,Oy~  riyO, 
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OxOy-  OyO, 

OxOy  -  OyO, 

0 

PxOy  "  PyO, 

Px^y  Py^x 
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.0 

0 
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(n) 

0018-9472/8 1/0600-0456500.75  ©1981  IEEE 


lilt  IHANSACTIONS  ON  SYSTIMS,  MAN.  AND  TYBIUNITICS,  VOl  ,  SMC-II,  NO.  6.  lUNE  1981 


457 


Treating  «,  o,  and  a  an  vectors  we  can  rewrite  this  in  terms  of  the 
vector  cross  product; 


0  (oXh)^  («X«),  (pXn)^ 

=  0  (.X.), 

(*><«),  (oXa)^  0  (pXa)^ 

0  0  0  0  . 


differential  translation  and  rotation  we  may  write: 


The  cross  products  of  orthogonal  unit  vectors  are  equal  to  other 
uiiit  vectors: 


In  the  case  of  a  prismatic  joint 


where  dq,  =  d6,  if  the  joint  is  revolute  and  dq^  =  dd,  if  the  joint  is 
prismatic.  Therefore  the  matrix  J,  the  Jacobian,  consists  of  six 
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de.- 

revolute 
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ip  X  0) 

•  pnimAiic 


0  0  0  n, 

^  0  0  0  D,  ^  ^  j 

"  0  0  0  a,.  "  ^  ’ 

.0  0  0  0. 

If  we  write  AT^  in  the  form  of  a  column  vector  representing  a 


where  n,  o,  a,  and  p  are  the  columns  of  U  where 

Un={A„»A„,^ . /<,).  (17) 

For  example,  using  the  values  of  U„  (62)-(79)  developed  for  the 
PUMA  arm  in  (2),  we  may  evaluate  the  expressions  given  for  the 
columns  of  the  Jacobian  and  obtain  the  Jacobian  matrix  for  this 
arm: 


{^4^13  +  <>3^23  +  CfSf,)  —  <^3(^23(04050^  —  S4  Si)  —  Si^S^Ci  ] 

(<^4^23  <*30-23  <*20'2)(  “■5'4C556  "b  0740^)  •“  <f  3  [  ~  C23(  C4C55’t  +  ^4C«)  +  ^23^5^6  ] 

(<^4^23  <*3023  <*202)(5’4S5)  —  <^3(0230455  +  523O5) 

“  [^23(040750115—  S4SI5)  +  023S’5Cl5] 

^23(0405515  +  54CI5)  +  CiiS^Si 

S23C4S5  —  02305 


{d4Sj+  a2){SiCi)  (  <^403  +  <1353 )(C4C50(5  S^S^) 

—  (1/453  +  <3303  +  <12)  (5^554  )  ■I'  (  ~  <^403  +  <J353)(C40554  +  54CI,) 
-  (1/453  +  0303  +  02)05-  (-4/403+  0353  >(0455) 

^405015  +  O454 

-540551,  +  O4CI5 

5455 


03(550^)  +  4/4(0405 CI5  5454)  0 

—03(5554)  —  ^4(0*0554+  54CI5)  0 

-0,05+4/4(0455)  0 

54C5O4  +  C^Si  ^sCs 

■“  54O5  54  +  O4O4  ^5^6 

^4^5  O5 
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Assuming  that  the  solution  his  been  evaluated,  the  computation 
of  the  Jacobian  requires  51  multiplies  and  24  additions.  Each 
solution  of  (16)  requires  36  multiplies  and  30  additions.  The 
earlier  method  of  Whitney  and  Klumpp  (3)  involves  an  additional 
108  multiplies  and  72  additions  to  compute  the  Jacobian 

Diifkrential  Change  in  Position 

In  manipulation  we  frequently  need  a  solution  to  (16),  that  is, 
given  a  desired  differential  change  in  position  and  orientation  </,, 
dy,  d.,  S,,  6,,,  and  6.  what  is  the  required  differential  change  in 
the  joint  coordinates  dtj,: 


It  is  sometimes  possible  to  invert  the  Jacobian  symbolically  but 
this  is  difficult  as  the  expression  of  the  elements  of  the  Jacobian 
(18)  are  quite  complicated.  A  numeric  solution  to  (16)  can  be 
obtained,  but  this  is  typically  far  too  slow  and  complicated  by  the 
Jacobian  becoming  singular  whenever  the  manipulator  becomes 
degenerate.  An  even  worse  approach  is  to  invert  the  Jacobian 
numerically  as  this  requires  six  solutions  to  (16).  The  approach 
we  will  follow  is  to  differentiate  the  solution  obtained  for  the 
joint  coordinates  given  a  value  of  7^  (2].  This  method  gives  us  , 
expressions  for  differential  changes  in  the  same  order  as  we 
obtained  the  joint  coordinates.  Manipulator  degeneracies  are 
easily  identified.  The  expressions  for  each  differential  change  in 
joint  coordinates  arc  a  function  of  change  in  Tj,  and  also  of  the 
differential  joint  angle  changes  already  obtained.  This  frequently 
results  in  simpler  expressions  for  the  differential  changes  and  if, 
due  to  joint  constraints,  a  change  is  not  possible,  then  the  change 
may  be  set  to  zero,  resulting  in  a  correct  solution  for  the  follow¬ 
ing  joints.  We  will  assume  that  a  solution  has  been  obtained  and 
that  the  sines  and  cosines  of  the  joint  angles  are  available. 

The  equations  relating  to  the  solution  for  the  PUMA  arm  are 
included  in  Appendix  1,  and  we  will  illustrate  the  method  by 
obtaim’ng  the  inverse  Jacobian  for  this  arm.  Equation  (80)  speci¬ 
fies  ffj,  for  the  PUMA  arm  which  we  may  differentiate  to  obtain 
an  expression  for  d0,: 

C^dp  —  Sidp 

^^>  =  -d-p+Tr-  -  (20) 

If  the  denominator  is  zero  the  manipulator  is  degenerate  and  d0^ 
may  be  a.ssigned  arbitrarily.  We  check,  as  we  obtain  each  dif¬ 
ferential  change,  in  case  d  dO  exceeds  the  joint  motion  limit.  If 
this  happens  we  set  d6  to 

d6  =  limit  —  0  (21 ) 

then  0  +  d0  equals  the  joint  motion  limit.  As  we  obtain  each 
differential  change  d0,,  we  also  evaluate  d{sin  0,)  and  t/fcos  9,)  as 

dS,  =  C,  d0,  (22) 

dC,=  -S,d0,.  (23) 

We  have  0,  specified  by  (81)  through  (83),  and  we  define 

V  =  2a2diSy+ 2a2a)Cj,  (24) 

differentiating,  we  obtain 

=  T  r  /  c  T  (25) 

2a2{dtCy- OySi) 

2/n(p)'//ii(p)  4  2/,,(p)<//,j(p)  (26) 


and  di^^{p)  and  rf/i^fp)  may  be  obtained  by  differentiating  (82) 
and  (83). 

We  now  solve  for  using  (84)-(86),  and  define 

"t>i= ‘^2/ii(p)  ~ ‘^iPi  (27) 

^2= ‘^i/ii(p)  + (28) 

Therefore 

■523*^2  4"  C23t)i  =  0.  (29) 

Differentiating,  we  obtain 

d0„  =  rsov 


^23*^2  ■523*^1 


where  dv^,  dcj,  r/tv,,  and  dtvj  may  be  explicitly  obtained  by  direct 
differentiation. 

Thus  * 


In  general,  if 


d02  —  d02i  —  d0^. 


iVsin  0 


Ncos  0 


where  N  is  any  variable  then 

N  cos  0d{N  sia  6)  —  N  s\a0d{  N  cos  0) 

d0  = - j - 5 - .  (33) 

(  hi  sin  0  )^  4-  {N  cos  0) 

From  (87)  and  (88),  we  obtain  expressions  for  NS^  and  NC^  and 
their  derivatives 

NSt=  C^a^  (34) 

d{NSt)=  -dS^a^-  S^da,  +  dC^ay+  C^da^  (35) 

A'Ca  =  C2)f74)  ~  S2ja,  (36) 

where 

©41=  CjQj,-*-  Sjfl^  (37) 

dD^^  —  dC\a^  +  C^da^A  dS^OyA  S-^doy  (38) 
so, 

d^NCf)  =  dC2)D,^  +  C2idDfi'-  dS2-^a,~  S2idai  (39) 
and  d0,  is  then  obtained  from  (33)  as 

^  NC^diNS^)-  NS^diNQ)  ,  , 

d0.  =  — i-i — i-' - M .  (40) 

{NS,f  +  {NC^f 

Once  again  a  zero  denominator  indicates  a  manipulator  degener¬ 
acy  and  d0t  may  then  be  assigned  arbitrarily. 

In  the  case  of  0^  we  have  expressions  for  both  sine  and  consine 
and  the  expression  for  the  derivative  becomes 

d0  =  C0d{S0)-  S0d(C0).  (41) 

When  we  have  expressions  for  both  the  sine  and  cosine  the 
manipulator  cannot  become  degenerate.  We  have  from  (89)  and 
(90)'. 

StNSt  (42) 

dSi=  dCtNCt+  C^d(NC^)  +  dStNSt+  S^d^NSt)  (43) 

Cj=  S23D414- C23n,  (44) 

t/Cj  =  </S2i/)4i  4- 52)dD4, -f  dC2ia,  4  C2jdaj.  (45) 

is  then  obtained  from  (33)  as 

d0i=  QdSi- S^dCi.  (46) 

In  this  case  we  do  not  need  to  evaluate  dS^  and  rfCj  as  they  are 

given  above. 


where 
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Finally,  for  we  have  from  (91)  and  (92): 

(47) 

Q  —  “^4^611"^  ^4^6112  (48) 

where 


t/i=7;  = 


"r 
.  0 


o, 

Oy 

O, 

0 


“> 

0 


P> 

P. 

1  . 


(67) 


^6111- ^10^+ (49) 
dN^^^^^=  t/C|<\  +  C^do,  +  dS^o^  +  S^dOy  (50) 

^6112  =  — 5|0^  +  C|0^  (51) 

-  S^do,  +  dC^Oy  +  C,do^  (52) 

^611  “  ^^23^6111  ~  ■^23®»  (53) 

‘^^611  “  “  ‘^^23^1  *■  ^23‘^<^2  (54) 

^612  ~  ~'5'23N4|||  —  C23O,  (55) 

dN^\-i  =  ~‘/S'23^6lll  “  '523'^^6MI  ~  ^^23^t  “  ^23‘^‘^l  (56) 

^61  “  C»^6II  ■S'4A(5||2  (57) 

‘^('4^611  4"  C»r/N(,ii  +  dS^N^^y2  4"  (58) 

then 

dS^  =  -dQN,,  -  C,dN,,  -  dSiN,,2  -  S,dN^,2  (59) 

dQ=  -dS,N,„-  S,dN,^^-i-  dC,N,tu+  C,d;V,„3  (60) 

and  dS^  is  obtained  from  (40)  as 

-  de,=  QdS,- S,dQ.  (61) 


The  differential  solution  dd/dT^  represents  91  multiplies  and 
55  additions  and  no  transcendentsd  function  calls,  assuming  that 
the  solution  has  been  evaluated. 

Summary 

The  Jacobian  can  be  developed  in  a  straightforward  manner 
from  kinematic  equations.  These  equations  and  the  Jacobian  can 
be  obtained  for  any  manipulator.  Further  if  the  solution  to  the 
kinematic  equations  has  been  obtained,  the  differential  change  in 
joint  coordinates  for  a  differential  change  in  position  and  orienta¬ 
tion  can  also  be  determined. 


Appendix 


The  kinematics  of  any  manipulator  are  defined  by  the  A 
matrices  (2].  From  the  matrices  we  develop  the  U  matrices  for  the 
PUMA  arm  as  follows: 


(62) 


QQ  ('S'Sj  S5  0 
■^sQ  ~SiS^  “Cj  0 

S*  c;  0  0 

.0  0  0  1 


(63) 


u,= 


('4C5Q 

54QQ+  QS, 
-S,Q  . 
0 


~  C^C^Sf,  —  SfCt,  CfSi  0 

~iS'4C55'i3 -I-  0464  0 

S5  Cj  d^ 

0  0  1 


(64) 


where 

=  c,ic23(C4CsQ-  5456)  -  •52355CJ  -  S,[S4C5q,  +  C4SJ 

(68) 

■S|[C23(C4QC;-  S’45’6)  -  S235sQ1  +  C,[54C5Q+  C4S6I 


(69) 

n^  —  ~S23(C4C5Q~  ^4^6)  “  6^23^5^  O^) 

~  ^l(“‘f-'23{OCs'56  "I"  5’4Q)  +  S'23S'5Sj] 

-S,[-S4C5S,-I-C4Q1  (71) 

o^.  —  S|(  — C23(C4C5S'4  +  SfCf,)  +  S2355'S’6l 

+  C,1-S4CsS’^-I-C4Q1  (72) 

Oj—  S23(C4C5.S’j  +  ■S4Q)  +  C23iS5.S4  (73) 

fl,  =  C,(C23C4S3  +  S23C5)  -  S,S4Sj  (74) 

=  S,(C23C4  Sj  +  S23C5)  -I-  C, S4S5  (75) 

4,=  -S23C4S5-I-C23CS  (76) 

Pa  ~  C|('^4'^23  43C23  +  02^2)  ~  ■^l'^3  (^^) 

Py~  ^t(^*^23  °3^23  4"  ^2^2)  4"  Cydy  (78) 

Pt~  ~(.~‘^4C23'^  OjS23'^  O2S2).  (79) 


The  solution  of  the  equations  represented  by  r6  is  presented  in 
(2),  the  key  equations  in  the  solution  are 


^iPa  4"  CiPy  ~  dy 

/ii(p)  4"  fni-P)  ~  d\~  Oy  —  a\  —  loyd^Sy  +  2a2®3C3 

/ii(p)  =  C^p,  -I-  S^py 

/i2(P)  =  -P, 

Sjy  _  H'2/fi(;>)  -  W|;7, 

Q3  »^i/ii(p)  4- WjP, 

*V,  =  OyCy  +  Oy 
W2  d  ^  O  ySy 

C4S5  =  C23(C|a,  -I-  SfOy)  —  SyyO, 

StSy=  ~S]aj,+  C^Oy 

^5=  C4lC23(C,a,-l-  S,a^)  -  SyyO,]  +  S4[-5,<7,  +  C.flJ 


(80) 

(81) 

(82) 

(83) 

(84) 

(85) 

(86) 

(87) 

(88) 
(89) 


Uy  = 


(/,= 


C3(C4QC|3—  54513)  —  SySyCf, 
54  5(3  )  +  C355Q 
54C5C13  +  C^Sf, 

0 

C23(Q6'5CJ3  “■  545^)  —  52355Q 
SyyiQCyQ-  S,S,)  +  CyySyQ 

54C5C13  +  C45(3 
0 


CyCfSy  +  53C5  d^Sy  -I-  a3C3 


-C3(C4C55^  +  54Q)  +  S,555e 
^Sy^C^CySf,  +  54Q)  —  C3555J  SyC^Sy  —  C3C5 
“54C55j+  C4Q  5455 

0  0 
~Q3(QQ'5'j+  54Q)  +  SyySyS^  C23C455+  523C5 
~523(C4Cj56+  54Q)  —  C2355513  523C455—  C23C5 

“  54C5  5j  +  54  Sy 

0  0 


-'dfCy  +  4353 


1 

^4^23  4“  43C23  +  OyCy 
”^4^23  4-  43533  +  OySy 

d3 

1 


(65) 


(66) 
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Cj  —  S23(^i‘^.<  )  (90) 

St=  -C5{C4lC23(C,0,  +  S,0,.)  -  5230j)  +  ■^4l“-S|t>j 

+  Q0,1)  +  •55{'^23(Qo.  +  +  G3OJ  (91) 

Q~  "■^4lC'23(QOj  +  S,0^.)  -  SijO.)  +  CeI-^jO^  +  C,0^].  (92) 
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of  properties. 

Cen-frAl  '  ^ro«ojw^  priced  on 

fire  +he  selec+ion.  of.  &.  s  I'toti  lo.ri^ 
err'fen'on.  c^tAci  ?  ■for»Mul<s.4fort.  <f-f 

a.  -^rmintx^on.  Iro  (e  . 

The  selection  6^  +iiese  ^fac-hrs  is 
sirart^l^^  inf/uesoceJ  b<j  -the 
<?f  scene  -h>  te  seom&>^'i~&o. 
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The  basic  piriyiciples  iloVolveJ  /Vl 

^racolyi^  ^ill  h-e  i)|os+rA“teJ  - 
<an  a^ppliCA-haiA^  In  FLXR  /rvic^^e  io.r<^€i: 
^e^yyien'i^'t  iorL ,  T'A'/s  /V  an  m^er^s^in^ 
praiUhn  because,  difjcren'L  re^/i>fiz  cx.rc 
usually  nod  soparoded  dranitdfans ^ 

“thus  }ryxa.hin^  dedcedian  less 

<5lK  i'Jeo.1  apfraoel^,  'T^resho/Ji'nQ  /s  ^Iso 
nai  use/l-  Suf-leJ  do  d(iis  jprchfet^  beh^ose 

d*^  (picol  yfartodjaf^  uaidbi'n.  o.  re^fOKu  . 


AJ^A^e^cy 

pp C n 

Re  ^i6n,  s4^ps  i-p^ 

6*-) /^e ^ /i>/>  ^ti>u>s  Tn-U  tmA^e  h^uindairy. 

(fj)  A  ptesei  numier  ^-f  f>/x^ts 

in.  A  ^ /yfen  m^e^si'fy  r^tnc^c  C€^'5. 
-hop  Zsy^  m*^c*o5iYy  sca/e)  h^ve 
beev\  ^rou^ffi, 

„N 

CC)  (Dp’J'imunn  S/je  /iaj 

re  AC  heJ 

Opht 'i^Ar^et  s/jje  as 

d'he  S'/je  Teoclne^  ojheto  ^rouy^ 

/i  tm) n\  iyiov^  Co ficep’f"  h  )(f us ’ir^sded 

IK  ^o I (ou>ii^^  ;^i^orG , 

.  .. .  •  ^., .. _ _  _ :  _ _ 


Pixel  Vafues 

/\/-/<rr  an  errixifc  %'Tar'i: ^  -the  ar^o, 
lincrease.s  <xi-  readies  <k 

pU'f  eau  ai -tkip  eJ'^e  c>-f  -f-Us  -har^ei: ^ 
Civ^d ’then  increases  tap'iJ/y  a.^Am^<^s 

hAekycuy\J  piy(e!s  he^i'n.  -fz>  eon-/ridvte 

’io 
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i 

K... 

%[  ZEn-flec-Z-torL  po'mi  may  he  (^-traciei 

hci  -m-e  Cur  Me  Ce.^. 

pg  .  a 

A  s-faTmi  Auere^^^  c^»-ffere^J- 

'  '^*  K 

^  "T^e  potn'i 

op-fj’wowo'*  -iar^ei  si'^je 
U5€*J  m -f^je  s^wppiw^  ro/®. 


i  . 


k'.-' 


^'.'• 

Ml 


tl 

t/ 

r.‘. 


Bd^fC  srruc-fure  of 

i)  ConsTruc-t  CL  his-To^raml p)y^e]-o.<^^\-ess 
±al,\e  . 

/}Jc/ Tosses 
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Ge<^/N  Oy  Oec //?/? //V^ 

j/-fg  pi'nST  PixEL  /A^  THB  T/^BLE  EirJ 

0  6T£Cr  /rS3  l4Ai//U<5  Ol^^eCT'  A;o. 

/T  Is  $C^A/fi/€C>  T^PPaui^J 

C^i  s  c.^a;  the 'T/)3Le  Jm  anbtfi  OEcnEft3(H<^ 
a/J  r^HrAjc^^  VP^LUES-  Suese^oei^T  PD^ELS 
Rr\E  /?  yUe-<AJ  <D0Te^C-r  ajo.  /  p: 

Ti^i^Y  Fz-ae^  Mar  fVh:Tf^c^f^T  Ta  /?-A/y 
^.x/5T/-va  <i^0  5“£=cr.  oTHaHUAi^sa  ^i-H&y 
pne  P5 pp^opfl / /rrs  oQ-Tccr 

i\'C, 

(?)  tViY^^V  S&P^^  P-TB 

aaTs-cT.s  n^acTftes  a  ptis-^srpau^/ie^ 

M  ^  -th^ 

OQTC-CTS  M  /  N  H-1  B/TE-J>.  ^£*10  piXSLS 
Vui-KCH  K;c:.c'C.D.  O^TrW 

v;~6^  OOT^CT.^  /r/ie  iQ/yJOi^Sh, 

XP  a  p/:r<S‘iL  aDT/>C£^AJT  7"^  Ma38<r 
0^5’  ^e»TeCT3 

o 

Prttf-v('»e»  TnaiR  C£>mb>^^-^  ft-^'i£=/? 

Dees  N:ar  tNaejrf^  -nte  MA-J'i'mcM  e^PecTEi 


ST^P  yoHBhl  /J^/y  OFTHB  Stc>Pp)aJ^ 
holes  S^T’ispi^b  F^a  BLL  /?c6/^MS 
SBI^S  6A6H/A^, 

BxGv^pUs  Oi  PLXfK 

•6icn. 

(a)  OtiQimAl  FLTH  /»7A^e 

Q))  BhneJ  n£>}sy 

(<?)  exAv^fle  e>f.  £,e^vt^9yirt-Aire>tf)  an 

"clean”  <5i*'d  nasy  »Vma^«s 
Se^w^^A'i’A'hoio  Test^t'k  am 

A-f4cy  T'f’  loas  snne>A~Hn^ 

^x'S  AVera-0»M^ 

0 

(jz)  S^^ianemi'A-hh^  resvl-h  ov>  blurred^ 
jOaisy  /wAi^e,  ioaa  a/so 

Slnnca-tl^  e</  j . 


HI 


RECO^MITIOiM  nAiO  THTBRPRBT^riCiM 


A 


Recoc^n^'T^c>l^^.  fs  b^^ica 

pr€>Q'?z^  FsS^  ^uinQ“h'^t% 

/ 

f^eco^iniTion.  /s  ~t<^ 

iJet'i4'sfy  €^<^ck  Se^me&rh^  ob^ec'f 

In  a  *rc>  ^SSi^n  C\. 

-4^  hr  ^€3acl^  eth^.  ) 

ZE ^^t(^4<sk4^oP\  (S  cl^$ely  htnk^d 
Loi'-hh  r^co^i^n^rf.at^'i  ^  i*^5  ^utnc^hhi^x 
-rs:?  ^\:5  n)  -/o  <si 

'ee<o^^i!^^eJ  C  lai^eioj )  5c^(/)e 


Set  o-f: 


e! 


.? 


,’'» 1  /O  *0'T-'’'^, 

»,->  I  H''  f  f  •■j^/  ^ 


~  e  J  of  teco^nt  ^/(O 

proceJores  5Ce/>tf  awd/y5;.s 

CCtOSiS^  of  -/-wo  loASlC  5^eps  ; 

CO  Sel^Mtn.  Of  «.  sef  of 
or  c/eserJ (p'^di'S  j  awt«J  C?)  ss  l<s<c-h^^ 
Of  a  e/ossr^»CA';feofio  strcA^^i^. 

\/o\ll  ConsiJer  ~the  proklcm  of 
$el«c.4-/h^  Jescri^'hors  i»  -ths 
liouttyt^  discussioy^j  okJ  -hnet^ 
u/,})  CowsiVer  'i^c.h  ni^uei  for 
>recOAi\.lhoin. 


Rea /anal  Deseri 

S  hex-' Iff  Jbc  --5  inJer pemoeyf-  i  a.-^ 
JiO‘^s'lh!e  -f''  \'a.rie'.i:t~c>i^  »w. 


5f  + 


U\e>\ 


OA'  vf  ra’^^oAto*^' 


Fourier  Descnp-rQrs 


Suppase  pa  i'vi'hs  on-  oau'n^a.r^ 


Reen  I 


"Fa Are  FPT  04  Qo^xoijr 

IH-  StT^g.  <5.>  e  srn*.  Jp**5  *>• 

:(■<?, iV>  PT 

Ri^“f^'y•^^Vl*'^  "i.^  yi0.ut4'\ 

W  <?  .j  ^ » 

/Mcl^p  <<?»>.<  I '?.*’/■  ji'^-  £>lfM^crr  ;^P££^V£^. 
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Ca 


rAvei 


bC-  lo 


t  {re^ 


BAc  ^we-S.£veM 


Wo-T^r  rP^^T 

loAS 


bc~% 


n-^  ^  is  cl'?'4  ^.  S 

7) 

p  a 

m  =  j  f 

O  mm 


rt'  TIix.  Cev'.'f  y'^  f  Yv\£}>n^ffr^^  Are.  de^-iri^r’o  ^-S 


^n 


U^',4-1 


^  J>  ^ 

-  ff  (X-  x)(<i)-^)jC5'i^)c(5<cl^ 


.  ^ 


Ur  }y)^/ 


^o 


Od 


HI 


iX  = 


~7  he  n£>ryy>^  t>  '^^r!  S 


^y«?  AlV^M 


^P| 


=  M 


n 


Ml 


r^pii+ , 


R  sei( 

^,  y-^,y>%l£ir/:/iif>K  ,  iVV^r?  \'€>'^'&.~hfh(fl.  fS 

cp  ^ri  4-ifi 

1 1  *ad  oi 

0 

-  SCdji+n,,)']  +  0'»(j-'^a3V 


:!A 

I  ^  ic)  f^irf^v-eJ .  ^) 

I  45"^  Re:iul4ft^^  i^€‘^TS  : 


1 

• 

Vasisfi 

6.2 

6. a 

•  ", 

n.£ 

/7.<d 

*v 

22.1 

29.S 

i 

12.^ 

2^.a 

1  jt 

*  '•* 
k*  • 

L  '*"< 

4S.7 

V?.3 

S 

1 ",' 

1  \ 

'-/. 

'  # 

3I.S 

32.1 

Ci 

tl  . 

VS.  6 

45.3 

Mir?©r 

5'' 

^3 

6.1 

6.3 

6. 3 

/7.1 

/6.  8 

•26.7 

22. 3 

f4.7 

26.1 

2  2.1 

Sl.l 

46.; 

4«s.S 

si.i 

32.0 

a^.s 

33.6 

46.0 

4^.S 

So 


p  ^  ~To  po  }c  ^ icA  I  -Desert pTo  ts 
^  T"opo/o^'/ ;  Tii<?  ■7-*oel'^ 
of  a  ^iflOVS  VuAu'^. 

®  1)^  Aefctmo:^'ioeK.  Vut'^LaiA  -featlyt^ 

§  ot-  4o'(ninA. 


Holes 

Mfi-f  Af-feHtrJ 

scalar 

c  Uoi  ng  e,  vci  fTii**^ 
loc  . 


S'l  ve-^^r  ,  fi4r, 
Conytec-I^J  Coi/rtpctAecfT 


—  CL  yeoiovK'  Uii 


itrl/y 


4W  p  t  f4if  cf.ifffji  of-  (4.3 
ptN.rvW^  OCJ>)  ^ 

Ct'VV'^  eit^tr^Lf  Tm  *fA«®' 


rOY"  Ck  n<9-r\A}crl<  Ct 

XPOj\oi>*-  repreSBA-if^ti  g^r<ai^li 

|m€  se^u't^et^'^s.  ) 

'^' tf'X- 

>v. 

£“=  VSI-  -j-  F 

V 


h  =  7-fl  +2  =  -2 
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Q-hket  Commonl^'UseJ  desct’tp-^fs 
si  iff  and  shape. 


I.  Acea*  fA) 


2.  Peti tn  e-iet*  Le  hn.'^'.h.  (P) 


S.  pVa  ~  A  r^easute  of  Cot»t  pod /less. 


of  PriVicip«/  c/a4ec  spreod 

(  efgPuvft/wes  amJ  eigcnvec+oi's  «f 


5.  Number  of  hole^,  ra-fib  of  hole  area, 
"ho  ob^'ec^  ateo. 


6  Relationships  betuieen  elei^enfs 
of  o.  polygcnai  apfiraitima-tion . 


7.  QounJi'na  hex  dimensions 

-fo  1.2  AM J  4'). 


Some,  deseti p-f-ots  o-f  posi'iion.  anj 
orien'f'&'hon 

I 

t.  CoorJi'na-fes  af  e«n-fer  of  ^rovWy 

Z.  CoOfJinciies  of  Confet-  of  hounding  bo)f 

3.  DlVec-^/on  of  ei^en-oxes 

4-.  Angles  of  vecfots  cen-fer  of 

orovi-fy  +«  cle&esi  anJ  furfhesr 
poirt'fs  in  perimefet" 


5n 

S.  DiVec-fron  cj- 

iwo  mosi  dls+^xni  points  m  ^he 
bou¥idat^y  of 


HOU^iH  TRAhiSFoaM 

Given,  m  poiWs  m Ciwmge) 

« 

plawe,  uj(‘sk  TO  ;f/V»cl  iubse4s 

-tkai  lie  on  a  STiraraljf  liffie. 

One  poss'i&le  solu4ion  is  *^e  firs'i 
find  all  /»  ej  bt^ 

6vear^  pair  poin'fs  AtiJ  'f&jen 
^I'wJ  All  Sufese-fs  -fka'f  ore  'close" 

“ho  paifficolar  limes. 

Tine  prol)levM  (o»+C\  proA®dore 

is  i't  fmVolvei  ^iiodiw^ 

hCn~i)l2  lines  Awd  -hUet\ 


v- 

,1 


I  ^ 


5( 


On'S  rXw-i)/^  ^‘‘ 

CovkKp&ri'sc&iS  tff  evey^  ^oiW  -f© 

A I )  -hU  e  I  ii/iAi'.  “rlri'is  is  Covt^pti^’o, 
4/  onAl/y  p  ro  I'li  bi^-iyje  ^oir  Vy\&S'f 
p  tro i/eiAos  prAC-ficai  €i^i9i-ft- 

Cat^ce. 


!  C 


L' 


t .  - 


K- 


►■i- 


Tke  prxfposed  h<^ 

av\d  YyioxJi-^ieJ  lou  buJa  4  <^^rs 

aroutoj  +iai.$  proof®»*i  by  usiw^ 

“hbe  (J-o  I  (otoiiiy  ifiormal  ye  presew  4^41^51 


4  all 


»ne 


I 


XtoiB  +  ysinfl  =  f 
Oi  0<TT 


"Tksa,  eac(f».  pdiVi-f  d^sc/t&es 

a.  sTnusoidal  Cory/e  |V»  ^-S 


Ps  X.CdS0+  ^.Sm  6 


yv  ^wirvje 

/  b'i 


ihe  key  •*  Corves 

Co yres pow^Ji'w^  *fo  Coliwe&f  |5<ssta‘» 

(m  pjdttC,  hdve  a  CovMtiMoDs  poswif 
Oj.  iwl^TS^  IVi  pl(^h<e 


TIius^  ~i~ke  pai'vfi  (^  ,  f  ) 

S-f  plavie  Je^t'tnes  -the  li^e 
XCosS^-t- si»  S^- f  p<&  sst^g  ta 

-rte  Colineor  |>o/W^.~T^e  prdi 


is>  yiOLo  "f"*  ;^iv^d»wg 

CcwcuSTst^v  Curves 


SiVnilfii'/y^  1,^  <Joa.|i-f^  ,  a  sei 
pomh 

-fw-e  6-f  |>/a»e  ow  -hhe  Cotrve 

frX^cose+  jj^siso© 

de’f'erMinff  dll  X-5  p/<a6»e 

DAss'm^  pdiM'f 


(  o^.  b^sic  prcper-fi^^ 

ef  ■^ke  l^ausk  "i  r&ns^orm. 

©A  poin'f  fw  "f^s  inoa^s  pjiat'ie 
CorreibonJs  4^o  a  sinosciJ^l  Cur^e 
fw  -f^ie  b^mweTej’  pfo-PiS. 

©A  p^jiw/  in  pfir&vne^ejr  p 

^  Corvss  pcnJs  ‘f’o  a  i^£tS 

-f^e  isviA^e  plane 
Poini's  on  a  S’!:  rai^hi  line  in. 
-f^€  p/ane  Correspefl^«/ 

'I'd  Corvei  -hktoo 
poini  in-l-lne  porAmeier  plane , 


rcm4s  otn 


€  s^e^e  cutM^ 


ifto  ^lasne 

poMc/  -fo  lines  -Hiroo^k  4’^e 
SCkme  p^W  m  -hke  i  ma^e. 
plane. 


^  .‘r 


||  zlm pibmbmta t/oa/ 


n:--- 


/•  Quaw4i^e  pQ.ro.nitt’feir  pUw£  iv>4'd 
c/i5(?re4e  c<zlls  C  la-f'ops*),  -S 


h.-' 


i 


&II  CiCCO\MolA-4or  Couio4s  4^ 


F^r  poiw'i  Com^oie 

P  =  C^zkdkB  -f*  ^  sin  IcA^ 

k-  ;... 

6v\i  fiocr^iMeia^  -^i^e  ce//s 

aJdy^sreJ  i>«2  p&i*« 

... 

3.  Exa.v»*iv>r  (?Awk  0.5.  ci«eui>»*ofcc49y.s 

CCelU)  j.  d  y  lo » ^  In  .  JT  ^ 

Cell  C^  "P  )  Cow*fA»V(5  C  CouM-/s, 

/*  J  *v 

■floei^  precisely  C  iWage  ]^c{\/fh 

he  a  I  OH  3  ■^'’  /iMf  pAva.tt^e'^eys 

fy  ("f"^  U»  i'f’ltiy\  4i»f  crrftr  o^4li>e  c  e  II.) 
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**7" ^  -hlrte  S  av'i^  is  df’vJJffc/  }tff^ 

K  ivj  CTC  S^'SW"^  3  ■? 


a.  7ii<s 


iwia  K-,  ivi  CYC  meHTS.  '4'hew  -§*7 

2.  * 

evevM  d>-rA‘i»  S€j 

VAiuf^  ^  ^  ^5>jrYesp<>t''c^‘“^^  4-rs 
,  p«S3*l  i/e  Value?  6. 

Siwce  "fi»ere  fiye  n  ^i^'ivfrs^ 

•i~kij  ivfvahes  n  ^C  Cont^po’Ta^'hoffiS ^ 
loVtic^  15  linear  in  Jt. 


/ 
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UNCLASSIFIED 


F/G  14/2 


NL 


5*0  p pose  "fSe  ^llotoiwa  O^ceo  wolok-lifiv 
values  resuH  ^rs>l*^  & 

(OJ-/4  n~iso. 


3» 

I?0 

fo 

p 

-io 
•Jo 

^  (50*/ncrewi®rt'fo} 

“T^e  ^^»'«3€  +4»o.'t  ^leMeJ  'H>»5 

Coio'^AiMs  o>.  lon^  '^zr-iric^ 

"pofyiih  oriew^J  m -f^a  oirecA^w 
d*^J  liC'f’bieetJi  /o^2o -f  r^ls^ 


iiW  ■f'<0 


.  ^Tvj  r-i 


f)  /yx  I  a 


o\/5-/-^w/j  t)  6>5  f  ‘^1  R 

•  ^eu  ns4fc 

•  Dec  tK  e^)  rei^  <'  c 

•  S  w  n'fAe.'H  c  C  s4)r  cjc4w ra  /  ) 


ti&oriSThc 


a4U 

p  er^'sy  m<aMce. 

'fLoA  m^er^rei 

oeA 

Ahco-h  ^ jp p r £>cx ci>L , 


7/ 


' ',  -  w  ■"  •  ^  * 


■v  r  ■■ 


7/ 

Dge>s/‘<o^_  t;/)  ^<D  i^  i*s  Sm 

QaseJ  oy\  oect's/i^^  ^ch&t^^s 
pA}rif6i<by7m^  pAT/®r<5^- 

space. 


Uj 

€> 

i 

•h 


M —  Jx^Ti'}^i>^J  } 
/  Ph^\).TYS 


■i—'  s^<^^r^:  h  '.yj 


f 

D&c/Sf£>fii.  3£>UV)J&tr^ 


b.' ' 


t.- 


7£ 


Corre  Iax  wyx 


■N 


\  K  ( 


1  ' 

^  . 

.•//■••/•I 

M 

r 

A-  //-“I 

•  1 

.'  t  1 

'V 


votsr.ij) 


i 


C--' 


■r. 

>  ■ 


^•v 

y 

kf 


WCy,«^)  M  +V>«  Le>ok\tn<^  ii>r  ^ 

yMA4e^^  l)e4ioAeu  *•  reAtaw 

S-fauJa^rJ  coi»ff(A.4>6iA 

»y% 

Wo/'ee^+o  .?y.4pw4 
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5  MorrtiAlrjfd  C6rrtfU4ii«»- -TAkw 

'$ A-c'^STt&rif'.z 


VW\  JO. 


'4 


£  fx^.\A..p(e:  .N.^‘.J<  ^^  Uihr  M 


V 


S^mh^k  GMi  -i  as 


Pa,'N’e 


rfi)s  are  rept^sen 


4^4  & 


I 


St^filrckciic  Va.ii  etn  R^c^ncfiaffi 

UnliLe-f'he  cJecisian.'-il^eote/ic  o.jj>^too.cL^ 
Su/i-fcicl tc  Yneiijocis 
"  ^4yuc4LJ  re**  £>{■  part-term  /n.  -fl)® 

dviJ  recoin'd i'c>fL  process^^- 


^riy>s  h’OAiM&jic. 


iCA/  , 


The  -i 


— 17 - 

Cluj  sj'rue-IvtAi  pcJr-teTyy  t^co^y)i-h^‘ 
are  used  -fo  Jencte  -me 

A 


appr^c^- 


7* 


.( 


Xn  -ihe  vec/s/an -tlfi  ee> re-ifc  aoprccxrl) 
p^x-tieriTs  Cite  re pre6e>»^e‘d 

nZn  H-ht  s^n-^Ac4?i:  cpprca<^^ 

<Sy^  re  »reseyi-ieJ  s^t>\l>^}s.~7hese 

ccr^  AttAmAed  pt  ■»-? 

C  -^Aivr  ^A5»c  i<^^s  ! 

•  s4rtfi^ 

•  /■ers 


Fet  : 


c 


c 

afec 
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n  •f’he  cle<t/s/i>A~'f:ke£>Ye-^rc  a. 


trie  a.j^>pT^c.ct 

pa<.-Herjos  are 
Ivrid  r?A/ifl5. 
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